/*-----------------------------------------------------------------------------

 Copyright 2017 Hopsan Group

    Licensed under the Apache License, Version 2.0 (the "License");
    you may not use this file except in compliance with the License.
    You may obtain a copy of the License at

        http://www.apache.org/licenses/LICENSE-2.0

    Unless required by applicable law or agreed to in writing, software
    distributed under the License is distributed on an "AS IS" BASIS,
    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
    See the License for the specific language governing permissions and
    limitations under the License.


 The full license is available in the file LICENSE.
 For details about the 'Hopsan Group' or information about Authors and
 Contributors see the HOPSANGROUP and AUTHORS files that are located in
 the Hopsan source code root directory.

-----------------------------------------------------------------------------*/

#ifndef AEROAIRCRAFT6DOFSS_HPP_INCLUDED
#define AEROAIRCRAFT6DOFSS_HPP_INCLUDED

#include <iostream>
#include "ComponentEssentials.h"
#include "ComponentUtilities.h"
#include "math.h"

//!
//! @file AeroAircraft6DOFSS.hpp
//! @author Petter Krus <petter.krus@liu.se>
//  co-author/auditor **Not yet audited by a second person**
//! @date Sun 28 Aug 2016 14:39:05
//! @brief Flight dynamics model of super-sonic aircraft
//! @ingroup AeroComponents
//!
//==This code has been autogenerated using Compgen==
//from 
/*{, C:, HopsanTrunk, componentLibraries, defaultLibrary, Special, \
AeroComponents}/AeroAircraft6DOFSSg26.nb*/

using namespace hopsan;

class AeroAircraft6DOFSS : public ComponentC
{
private:
     double afin;
     double an1;
     double an2;
     double ap1;
     double ap2;
     double AR1;
     double AR2;
     double ARfin;
     double Cd01;
     double Cd02;
     double Cd0b;
     double Cd0fin;
     double CdW0;
     double CLalpha1;
     double CLalpha2;
     double CLalphabh;
     double CLalphabv;
     double CLalphafin;
     double CLde1;
     double CLde12;
     double Cdide1;
     double Cdide12;
     double Cdide112;
     double de10;
     double de120;
     double dM;
     double Cm01;
     double Cmfs1;
     double Cmde1;
     double Cmde12;
     double CLdefin;
     double Cydeelev;
     double Cnbeta;
     double dah1;
     double dah2;
     double en;
     double e1;
     double e2;
     double efin;
     double epsM;
     double awfin;
     double awn1;
     double awn2;
     double awp1;
     double awp2;
     double gamma1;
     double gamma2;
     double hthrust0;
     double ia1;
     double ia2;
     double Ix0;
     double Ixz0;
     double Iy0;
     double Iz0;
     double lambda1;
     double lambda2;
     double lambdafin;
     double lc10;
     double lc20;
     double lc120;
     double lcfin0;
     double Me;
     double mac0;
     double rc10;
     double rc120;
     double rc20;
     double rcfin0;
     double S1;
     double S20;
     double Sbh0;
     double Sbv0;
     double Sfin0;
     double xbach0;
     double xbacv0;
     double xbcge0;
     double xcargo0;
     double xfuel0;
     double xw10;
     double xw20;
     double xwfin0;
     double xeng0;
     double yeng0;
     double g0;
     double kground;
     double cground;
     Port *mpPal1;
     Port *mpPar1;
     Port *mpPal12;
     Port *mpPar12;
     Port *mpPal2;
     Port *mpPar2;
     Port *mpPfin;
     double delayParts1[9];
     double delayParts2[9];
     double delayParts3[9];
     double delayParts4[9];
     double delayParts5[9];
     double delayParts6[9];
     double delayParts7[9];
     double delayParts8[9];
     double delayParts9[9];
     double delayParts10[9];
     double delayParts11[9];
     double delayParts12[9];
     double delayParts13[9];
     Matrix jacobianMatrix;
     Vec systemEquations;
     Matrix delayedPart;
     int i;
     int iter;
     int mNoiter;
     double jsyseqnweight[4];
     int order[13];
     int mNstep;
     //Port Pal1 variable
     double toral1;
     double thetaal1;
     double wal1;
     double cal1;
     double Zcal1;
     double eqInertiaal1;
     //Port Par1 variable
     double torar1;
     double thetaar1;
     double war1;
     double car1;
     double Zcar1;
     double eqInertiaar1;
     //Port Pal12 variable
     double toral12;
     double thetaal12;
     double wal12;
     double cal12;
     double Zcal12;
     double eqInertiaal12;
     //Port Par12 variable
     double torar12;
     double thetaar12;
     double war12;
     double car12;
     double Zcar12;
     double eqInertiaar12;
     //Port Pal2 variable
     double toral2;
     double thetaal2;
     double wal2;
     double cal2;
     double Zcal2;
     double eqInertiaal2;
     //Port Par2 variable
     double torar2;
     double thetaar2;
     double war2;
     double car2;
     double Zcar2;
     double eqInertiaar2;
     //Port Pfin variable
     double torfin;
     double thetafin;
     double wfin;
     double cfin;
     double Zcfin;
     double eqInertiafin;
//==This code has been autogenerated using Compgen==
     //inputVariables
     double deCD;
     double deCY;
     double deCL;
     double deCl;
     double deCm;
     double deCn;
     double thrustl;
     double thrustr;
     double dezthrustl;
     double dezthrustr;
     double deythrustl;
     double deythrustr;
     double Mfuel;
     double Mcargo;
     double rho;
     double vM;
     double vturbx;
     double vturby;
     double vturbz;
     double wturbx;
     double wturby;
     double wturbz;
     //outputVariables
     double xcg;
     double ycg;
     double zcg;
     double vx;
     double vy;
     double vz;
     double Psi;
     double Thetao;
     double Phi;
     double Ub;
     double Vb;
     double Wb;
     double v;
     double Pb;
     double Qb;
     double Rb;
     double q0;
     double q1;
     double q2;
     double q3;
     double AlphaAttack;
     double BetaSlip;
     double altitude;
     double gfx;
     double gfy;
     double gfz;
     double CL1;
     double Cd1;
     double CL2;
     double Cd2;
     double CLtot;
     double Cdtot;
     double Lift1;
     double Lift2;
     double Fax;
     double Faz;
     //InitialExpressions variables
     //LocalExpressions variables
     double b1;
     double b2;
     double smc;
     double mac;
     double hthrust;
     double Ix;
     double Ixz;
     double Iy;
     double Iz;
     double lc1;
     double lc2;
     double lc12;
     double lcfin;
     double rc1;
     double rc12;
     double rc2;
     double rcfin;
     double S2;
     double Sbh;
     double Sbv;
     double Sfin;
     double xbach;
     double xbacv;
     double xbcge;
     double xcargo;
     double xfuel;
     double xw1;
     double xw2;
     double xwfin;
     double xeng;
     double yeng;
     double betaM;
     double CLalpha1e;
     double CLalpha2e;
     double e1e;
     double e2e;
     double Alpha;
     double qpress;
     double Beta;
     double mass;
     double xbcg;
     double Dragl1;
     double Dragr1;
     double Liftl1;
     double Liftr1;
     double Dragl2;
     double Dragr2;
     double Liftl2;
     double Liftr2;
     double Liftb;
     double Dragb;
     double Cfin;
     double Dragfin;
     double Mdvtheta;
     double Mdvpsi;
     double Fx;
     double Fy;
     double Fz;
     double Lb;
     double Mb;
     double Nb;
     //Expressions variables
     //Port Pal1 pointer
     double *mpP_toral1;
     double *mpP_thetaal1;
     double *mpP_wal1;
     double *mpP_cal1;
     double *mpP_Zcal1;
     double *mpP_eqInertiaal1;
     //Port Par1 pointer
     double *mpP_torar1;
     double *mpP_thetaar1;
     double *mpP_war1;
     double *mpP_car1;
     double *mpP_Zcar1;
     double *mpP_eqInertiaar1;
     //Port Pal12 pointer
     double *mpP_toral12;
     double *mpP_thetaal12;
     double *mpP_wal12;
     double *mpP_cal12;
     double *mpP_Zcal12;
     double *mpP_eqInertiaal12;
     //Port Par12 pointer
     double *mpP_torar12;
     double *mpP_thetaar12;
     double *mpP_war12;
     double *mpP_car12;
     double *mpP_Zcar12;
     double *mpP_eqInertiaar12;
     //Port Pal2 pointer
     double *mpP_toral2;
     double *mpP_thetaal2;
     double *mpP_wal2;
     double *mpP_cal2;
     double *mpP_Zcal2;
     double *mpP_eqInertiaal2;
     //Port Par2 pointer
     double *mpP_torar2;
     double *mpP_thetaar2;
     double *mpP_war2;
     double *mpP_car2;
     double *mpP_Zcar2;
     double *mpP_eqInertiaar2;
     //Port Pfin pointer
     double *mpP_torfin;
     double *mpP_thetafin;
     double *mpP_wfin;
     double *mpP_cfin;
     double *mpP_Zcfin;
     double *mpP_eqInertiafin;
     //Delay declarations
//==This code has been autogenerated using Compgen==
     //inputVariables pointers
     double *mpdeCD;
     double *mpdeCY;
     double *mpdeCL;
     double *mpdeCl;
     double *mpdeCm;
     double *mpdeCn;
     double *mpthrustl;
     double *mpthrustr;
     double *mpdezthrustl;
     double *mpdezthrustr;
     double *mpdeythrustl;
     double *mpdeythrustr;
     double *mpMfuel;
     double *mpMcargo;
     double *mprho;
     double *mpvM;
     double *mpvturbx;
     double *mpvturby;
     double *mpvturbz;
     double *mpwturbx;
     double *mpwturby;
     double *mpwturbz;
     //inputParameters pointers
     double *mpafin;
     double *mpan1;
     double *mpan2;
     double *mpap1;
     double *mpap2;
     double *mpAR1;
     double *mpAR2;
     double *mpARfin;
     double *mpCd01;
     double *mpCd02;
     double *mpCd0b;
     double *mpCd0fin;
     double *mpCdW0;
     double *mpCLalpha1;
     double *mpCLalpha2;
     double *mpCLalphabh;
     double *mpCLalphabv;
     double *mpCLalphafin;
     double *mpCLde1;
     double *mpCLde12;
     double *mpCdide1;
     double *mpCdide12;
     double *mpCdide112;
     double *mpde10;
     double *mpde120;
     double *mpdM;
     double *mpCm01;
     double *mpCmfs1;
     double *mpCmde1;
     double *mpCmde12;
     double *mpCLdefin;
     double *mpCydeelev;
     double *mpCnbeta;
     double *mpdah1;
     double *mpdah2;
     double *mpen;
     double *mpe1;
     double *mpe2;
     double *mpefin;
     double *mpepsM;
     double *mpawfin;
     double *mpawn1;
     double *mpawn2;
     double *mpawp1;
     double *mpawp2;
     double *mpgamma1;
     double *mpgamma2;
     double *mphthrust0;
     double *mpia1;
     double *mpia2;
     double *mpIx0;
     double *mpIxz0;
     double *mpIy0;
     double *mpIz0;
     double *mplambda1;
     double *mplambda2;
     double *mplambdafin;
     double *mplc10;
     double *mplc20;
     double *mplc120;
     double *mplcfin0;
     double *mpMe;
     double *mpmac0;
     double *mprc10;
     double *mprc120;
     double *mprc20;
     double *mprcfin0;
     double *mpS1;
     double *mpS20;
     double *mpSbh0;
     double *mpSbv0;
     double *mpSfin0;
     double *mpxbach0;
     double *mpxbacv0;
     double *mpxbcge0;
     double *mpxcargo0;
     double *mpxfuel0;
     double *mpxw10;
     double *mpxw20;
     double *mpxwfin0;
     double *mpxeng0;
     double *mpyeng0;
     double *mpg0;
     double *mpkground;
     double *mpcground;
     //outputVariables pointers
     double *mpxcg;
     double *mpycg;
     double *mpzcg;
     double *mpvx;
     double *mpvy;
     double *mpvz;
     double *mpPsi;
     double *mpThetao;
     double *mpPhi;
     double *mpUb;
     double *mpVb;
     double *mpWb;
     double *mpv;
     double *mpPb;
     double *mpQb;
     double *mpRb;
     double *mpq0;
     double *mpq1;
     double *mpq2;
     double *mpq3;
     double *mpAlphaAttack;
     double *mpBetaSlip;
     double *mpaltitude;
     double *mpgfx;
     double *mpgfy;
     double *mpgfz;
     double *mpCL1;
     double *mpCd1;
     double *mpCL2;
     double *mpCd2;
     double *mpCLtot;
     double *mpCdtot;
     double *mpLift1;
     double *mpLift2;
     double *mpFax;
     double *mpFaz;
     Delay mDelayedPart10;
     Delay mDelayedPart11;
     Delay mDelayedPart20;
     Delay mDelayedPart21;
     Delay mDelayedPart30;
     Delay mDelayedPart31;
     Delay mDelayedPart40;
     Delay mDelayedPart41;
     Delay mDelayedPart50;
     Delay mDelayedPart51;
     Delay mDelayedPart60;
     Delay mDelayedPart61;
     Delay mDelayedPart70;
     Delay mDelayedPart71;
     Delay mDelayedPart80;
     Delay mDelayedPart81;
     Delay mDelayedPart90;
     Delay mDelayedPart91;
     Delay mDelayedPart100;
     Delay mDelayedPart101;
     Delay mDelayedPart110;
     Delay mDelayedPart111;
     Delay mDelayedPart120;
     Delay mDelayedPart121;
     Delay mDelayedPart130;
     Delay mDelayedPart131;
     EquationSystemSolver *mpSolver;

public:
     static Component *Creator()
     {
        return new AeroAircraft6DOFSS();
     }

     void configure()
     {
//==This code has been autogenerated using Compgen==

        mNstep=9;
        jacobianMatrix.create(13,13);
        systemEquations.create(13);
        delayedPart.create(14,6);
        mNoiter=2;
        jsyseqnweight[0]=1;
        jsyseqnweight[1]=0.67;
        jsyseqnweight[2]=0.5;
        jsyseqnweight[3]=0.5;


        //Add ports to the component
        mpPal1=addPowerPort("Pal1","NodeMechanicRotational");
        mpPar1=addPowerPort("Par1","NodeMechanicRotational");
        mpPal12=addPowerPort("Pal12","NodeMechanicRotational");
        mpPar12=addPowerPort("Par12","NodeMechanicRotational");
        mpPal2=addPowerPort("Pal2","NodeMechanicRotational");
        mpPar2=addPowerPort("Par2","NodeMechanicRotational");
        mpPfin=addPowerPort("Pfin","NodeMechanicRotational");
        //Add inputVariables to the component
            addInputVariable("deCD","change in drag ceff","",0.,&mpdeCD);
            addInputVariable("deCY","change in side force \
ceff","",0.,&mpdeCY);
            addInputVariable("deCL","change in lift ceff","",0.,&mpdeCL);
            addInputVariable("deCl","change in x-mom coeff","",0.,&mpdeCl);
            addInputVariable("deCm","change in y-mom coeff","",0.,&mpdeCm);
            addInputVariable("deCn","change in z-mom coeff","",0.,&mpdeCn);
            addInputVariable("thrustl","Left engine \
thrust","N",0.,&mpthrustl);
            addInputVariable("thrustr","Right engine \
thrust","N",0.,&mpthrustr);
            addInputVariable("dezthrustl","Left z-thrust \
angle","rad",0.,&mpdezthrustl);
            addInputVariable("dezthrustr","Right z-thrust \
angle","rad",0.,&mpdezthrustr);
            addInputVariable("deythrustl","Left y-hrust \
angle","rad",0.,&mpdeythrustl);
            addInputVariable("deythrustr","Right y-thrust \
angle","rad",0.,&mpdeythrustr);
            addInputVariable("Mfuel","Fuel weight","kg",0.,&mpMfuel);
            addInputVariable("Mcargo","Cargo weight","kg",0.,&mpMcargo);
            addInputVariable("rho","Air density","kg/m3",1.25,&mprho);
            addInputVariable("vM","Speed of sound","m/s",340.,&mpvM);
            addInputVariable("vturbx","wind x","m/s",0.,&mpvturbx);
            addInputVariable("vturby","wind y","m/s",0.,&mpvturby);
            addInputVariable("vturbz","wind z","m/s",0.,&mpvturbz);
            addInputVariable("wturbx","wind rotation \
x","rad/s",0.,&mpwturbx);
            addInputVariable("wturby","wind rotation \
y","rad/s",0.,&mpwturby);
            addInputVariable("wturbz","wind rotation \
z","rad/s",0.,&mpwturbz);

        //Add inputParammeters to the component
            addInputVariable("afin", "break angle 1", "rad", 0.3,&mpafin);
            addInputVariable("an1", "Neg. break angle 1", "rad", 0.6,&mpan1);
            addInputVariable("an2", "Neg. break angle 2", "rad", 0.6,&mpan2);
            addInputVariable("ap1", "Pos. break angle 1", "rad", 0.9,&mpap1);
            addInputVariable("ap2", "Pos. break angle 2", "rad", 0.7,&mpap2);
            addInputVariable("AR1", "Aspect ratio 1", "", 3.62,&mpAR1);
            addInputVariable("AR2", "Aspect ratio 2", "", 3.62,&mpAR2);
            addInputVariable("ARfin", "Aspect ratio fin", "", 1.5,&mpARfin);
            addInputVariable("Cd01", "Drag coef. 1", "", 0.0045,&mpCd01);
            addInputVariable("Cd02", "Drag coef. 2", "", 0.0045,&mpCd02);
            addInputVariable("Cd0b", "Drag coef. body", "", 0.03,&mpCd0b);
            addInputVariable("Cd0fin", "Drag coef. fin", "", \
0.0045,&mpCd0fin);
            addInputVariable("CdW0", "Wave drag coef.", "", 0.048,&mpCdW0);
            addInputVariable("CLalpha1", "L. slope coef. 1", "", \
2.1,&mpCLalpha1);
            addInputVariable("CLalpha2", "L. slope coef. 2", "", \
2.2,&mpCLalpha2);
            addInputVariable("CLalphabh", "L. slope c. body h", "", \
2.,&mpCLalphabh);
            addInputVariable("CLalphabv", "L. slope c. bodyv", "", \
2.,&mpCLalphabv);
            addInputVariable("CLalphafin", "L. sl. c. fin", "", \
0.8,&mpCLalphafin);
            addInputVariable("CLde1", "Ctrl surface coef 1", "", \
0.1,&mpCLde1);
            addInputVariable("CLde12", "Flap rudder coef 1", "", \
0.2,&mpCLde12);
            addInputVariable("Cdide1", "Airleron drag coef 1", "", \
0.,&mpCdide1);
            addInputVariable("Cdide12", "Flap drag coef 1", "", \
0.,&mpCdide12);
            addInputVariable("Cdide112", "Flap rudder cross drag coef 1", "", \
0.,&mpCdide112);
            addInputVariable("de10", "rudder min drag angle 1", "", \
0.01,&mpde10);
            addInputVariable("de120", "Flap min drag angle 1", "", \
0.01,&mpde120);
            addInputVariable("dM", "Width of transonic region (rel. Mach)", \
"", 0.1,&mpdM);
            addInputVariable("Cm01", "Mom coeff. wing 1", "", -0.1,&mpCm01);
            addInputVariable("Cmfs1", "Mom coeff.1, fully separated", "", \
-0.5,&mpCmfs1);
            addInputVariable("Cmde1", "Mom slop coeff 1", "", 0.02,&mpCmde1);
            addInputVariable("Cmde12", "Flap Mom slop coeff 1", "", \
0.1,&mpCmde12);
            addInputVariable("CLdefin", "Rudder coef 1", "", \
0.0827084,&mpCLdefin);
            addInputVariable("Cydeelev", "elevator side force coef", "", \
0.1,&mpCydeelev);
            addInputVariable("Cnbeta", "sideslip z-axsis moment coef", "", \
0,&mpCnbeta);
            addInputVariable("dah1", "down wash effect on 1", "", \
1.,&mpdah1);
            addInputVariable("dah2", "down wash effect on 2", "", \
1.,&mpdah2);
            addInputVariable("en", "e", "", 2.71828,&mpen);
            addInputVariable("e1", "Osw. effic. factor 1", "", 0.95,&mpe1);
            addInputVariable("e2", "Osw. effic. factor 21", "", 0.95,&mpe2);
            addInputVariable("efin", "Osw. eff. f. fin", "", 0.95,&mpefin);
            addInputVariable("epsM", "Mach smoothening factor", "", \
0.1,&mpepsM);
            addInputVariable("awfin", "CL exponent fin", "", 0.2,&mpawfin);
            addInputVariable("awn1", "CL exponent neg. 1", "", 0.2,&mpawn1);
            addInputVariable("awn2", "CL exponent neg. 2", "", 0.2,&mpawn2);
            addInputVariable("awp1", "CL exponent pos 1", "", 0.2,&mpawp1);
            addInputVariable("awp2", "CL exponent neg 1", "", 0.2,&mpawp2);
            addInputVariable("gamma1", "dehidral", "rad", \
-0.0872665,&mpgamma1);
            addInputVariable("gamma2", "dehidral", "rad", \
-0.0872665,&mpgamma2);
            addInputVariable("hthrust0", "engine vert. pos", "", \
0.,&mphthrust0);
            addInputVariable("ia1", "incidence angle 1", "rad", 0.,&mpia1);
            addInputVariable("ia2", "incidence angle 2", " rad", \
0.02,&mpia2);
            addInputVariable("Ix0", "Norm. Inertia moment Ix/(Me S1)", " ", \
0.0147,&mpIx0);
            addInputVariable("Ixz0", "Norm. Inertia moment", " ", \
0.0055,&mpIxz0);
            addInputVariable("Iy0", "Norm. Inertia moment", " ", \
1.131,&mpIy0);
            addInputVariable("Iz0", "Inertia moment", " ", 1.279,&mpIz0);
            addInputVariable("lambda1", "sweep 1", "rad", \
0.436332,&mplambda1);
            addInputVariable("lambda2", "sweep 2", "rad", \
0.436332,&mplambda2);
            addInputVariable("lambdafin", "sweep fin", "rad", \
0.785398,&mplambdafin);
            addInputVariable("lc10", "norm. ctrl surf. 1 ac fr hinge \
lc1/sqrt(AR1 S1)", "", 0.01,&mplc10);
            addInputVariable("lc20", "norm. ctrl surf. 2 ac fr hinge \
lc1/sqrt(AR1 S1)", "", 0.05,&mplc20);
            addInputVariable("lc120", "norm. flap 1 ac fr hinge", "", \
0.01,&mplc120);
            addInputVariable("lcfin0", "ctrl s. fin ac fr hinge", "", \
0.01,&mplcfin0);
            addInputVariable("Me", "Empty weight", "kg", 8700.,&mpMe);
            addInputVariable("mac0", "mean aerodynamic cord/Sqrt(S1/b1)", "", \
1.,&mpmac0);
            addInputVariable("rc10", "norm. ctrl surface 1 mom. arm", "", \
0.25,&mprc10);
            addInputVariable("rc120", "norm. ctrl surface 12 mom. arm", "", \
0.25,&mprc120);
            addInputVariable("rc20", "norm. ctrl surface 1 mom. arm", "", \
0.15,&mprc20);
            addInputVariable("rcfin0", "norm. ctrl surf. fin mom. arm", "", \
0.1,&mprcfin0);
            addInputVariable("S1", "wing area 1", "m2", 27.,&mpS1);
            addInputVariable("S20", "norm. wing area 2", "", 0.36,&mpS20);
            addInputVariable("Sbh0", "norm. hor. proj. area", "", \
0.2,&mpSbh0);
            addInputVariable("Sbv0", "norm.body vert. proj. area", "", \
0.1,&mpSbv0);
            addInputVariable("Sfin0", "norm. fin area", "", 0.17,&mpSfin0);
            addInputVariable("xbach0", "norm. body ac. hor.", "", \
3,&mpxbach0);
            addInputVariable("xbacv0", "norm. body ac vert.", " ", \
3,&mpxbacv0);
            addInputVariable("xbcge0", "norm. body cg", " ", 3,&mpxbcge0);
            addInputVariable("xcargo0", "norm. cargo pos.", " ", \
3,&mpxcargo0);
            addInputVariable("xfuel0", "", " ", 3,&mpxfuel0);
            addInputVariable("xw10", "norm. wing1  position", " ", \
3,&mpxw10);
            addInputVariable("xw20", "norm. wing 2 position", " ", \
4.8,&mpxw20);
            addInputVariable("xwfin0", "norm. fin position", "", \
4.8,&mpxwfin0);
            addInputVariable("xeng0", "norm. fin position", "", \
4.8,&mpxeng0);
            addInputVariable("yeng0", "engines off. from center", "", \
0.,&mpyeng0);
            addInputVariable("g0", "Gravity acceleration", "m/s^2", \
9.81,&mpg0);
            addInputVariable("kground", "Ground stiffness (for limitiation)", \
"N/m", 10000.,&mpkground);
            addInputVariable("cground", "Ground damping (for limitiation)", \
"Ns/m", 1000.,&mpcground);
        //Add outputVariables to the component
            addOutputVariable("xcg","Horizontal position 1","m",0,&mpxcg);
            addOutputVariable("ycg","Horizontal position 2","m",0,&mpycg);
            addOutputVariable("zcg","Vertical position","m",0,&mpzcg);
            addOutputVariable("vx","Horizontal speed 1","m",0,&mpvx);
            addOutputVariable("vy","Horizontal speed 2","m",0,&mpvy);
            addOutputVariable("vz","Vertical speed","m",0,&mpvz);
            addOutputVariable("Psi","Azimuth angle","rad",0,&mpPsi);
            addOutputVariable("Thetao","Elevation angle","rad",0,&mpThetao);
            addOutputVariable("Phi","Bank angle","rad",0,&mpPhi);
            addOutputVariable("Ub","Speed xb-axis","m/s",100,&mpUb);
            addOutputVariable("Vb","Speed yb-axis","m/s",0,&mpVb);
            addOutputVariable("Wb","Speed zb-axis","m/s",0,&mpWb);
            addOutputVariable("v","Speed","m/s",0,&mpv);
            addOutputVariable("Pb","Angular velocity","rad/s",0,&mpPb);
            addOutputVariable("Qb","Angular velocity","rad/s",0,&mpQb);
            addOutputVariable("Rb","Angular velocity","rad/s",0,&mpRb);
            addOutputVariable("q0","quartenion 0","",0,&mpq0);
            addOutputVariable("q1","quartenion 1","",0,&mpq1);
            addOutputVariable("q2","quartenion 2","",0,&mpq2);
            addOutputVariable("q3","quartenion 3","",0,&mpq3);
            addOutputVariable("AlphaAttack","Angle of \
atack","rad",0,&mpAlphaAttack);
            addOutputVariable("BetaSlip","Sideslip \
angle","rad/s",0,&mpBetaSlip);
            addOutputVariable("altitude","altitude","m",0,&mpaltitude);
            addOutputVariable("gfx","g-force in x","m/s^2",0,&mpgfx);
            addOutputVariable("gfy","g-force in y","m/s^2",0,&mpgfy);
            addOutputVariable("gfz","g-force in z","m/s^2",0,&mpgfz);
            addOutputVariable("CL1","Lift coeff. wing 1","",0,&mpCL1);
            addOutputVariable("Cd1","Drag coeff. wing 1","",0,&mpCd1);
            addOutputVariable("CL2","Lift coeff. wing 2","",0,&mpCL2);
            addOutputVariable("Cd2","Drag coeff. wing 2","",0,&mpCd2);
            addOutputVariable("CLtot","Total lift coeff.","",0,&mpCLtot);
            addOutputVariable("Cdtot","Total drag coeff.","",0,&mpCdtot);
            addOutputVariable("Lift1","Lift from wing 1","N",0,&mpLift1);
            addOutputVariable("Lift2","Lift from wing 2","N",0,&mpLift2);
            addOutputVariable("Fax","Aero force in z","N",0,&mpFax);
            addOutputVariable("Faz","Aero force in x","N",0,&mpFaz);

//==This code has been autogenerated using Compgen==
        //Add constantParameters
        mpSolver = new EquationSystemSolver(this,13);
     }

    void initialize()
     {
        //Read port variable pointers from nodes
        //Port Pal1
        mpP_toral1=getSafeNodeDataPtr(mpPal1, \
NodeMechanicRotational::Torque);
        mpP_thetaal1=getSafeNodeDataPtr(mpPal1, \
NodeMechanicRotational::Angle);
        mpP_wal1=getSafeNodeDataPtr(mpPal1, \
NodeMechanicRotational::AngularVelocity);
        mpP_cal1=getSafeNodeDataPtr(mpPal1, \
NodeMechanicRotational::WaveVariable);
        mpP_Zcal1=getSafeNodeDataPtr(mpPal1, \
NodeMechanicRotational::CharImpedance);
        mpP_eqInertiaal1=getSafeNodeDataPtr(mpPal1, \
NodeMechanicRotational::EquivalentInertia);
        //Port Par1
        mpP_torar1=getSafeNodeDataPtr(mpPar1, \
NodeMechanicRotational::Torque);
        mpP_thetaar1=getSafeNodeDataPtr(mpPar1, \
NodeMechanicRotational::Angle);
        mpP_war1=getSafeNodeDataPtr(mpPar1, \
NodeMechanicRotational::AngularVelocity);
        mpP_car1=getSafeNodeDataPtr(mpPar1, \
NodeMechanicRotational::WaveVariable);
        mpP_Zcar1=getSafeNodeDataPtr(mpPar1, \
NodeMechanicRotational::CharImpedance);
        mpP_eqInertiaar1=getSafeNodeDataPtr(mpPar1, \
NodeMechanicRotational::EquivalentInertia);
        //Port Pal12
        mpP_toral12=getSafeNodeDataPtr(mpPal12, \
NodeMechanicRotational::Torque);
        mpP_thetaal12=getSafeNodeDataPtr(mpPal12, \
NodeMechanicRotational::Angle);
        mpP_wal12=getSafeNodeDataPtr(mpPal12, \
NodeMechanicRotational::AngularVelocity);
        mpP_cal12=getSafeNodeDataPtr(mpPal12, \
NodeMechanicRotational::WaveVariable);
        mpP_Zcal12=getSafeNodeDataPtr(mpPal12, \
NodeMechanicRotational::CharImpedance);
        mpP_eqInertiaal12=getSafeNodeDataPtr(mpPal12, \
NodeMechanicRotational::EquivalentInertia);
        //Port Par12
        mpP_torar12=getSafeNodeDataPtr(mpPar12, \
NodeMechanicRotational::Torque);
        mpP_thetaar12=getSafeNodeDataPtr(mpPar12, \
NodeMechanicRotational::Angle);
        mpP_war12=getSafeNodeDataPtr(mpPar12, \
NodeMechanicRotational::AngularVelocity);
        mpP_car12=getSafeNodeDataPtr(mpPar12, \
NodeMechanicRotational::WaveVariable);
        mpP_Zcar12=getSafeNodeDataPtr(mpPar12, \
NodeMechanicRotational::CharImpedance);
        mpP_eqInertiaar12=getSafeNodeDataPtr(mpPar12, \
NodeMechanicRotational::EquivalentInertia);
        //Port Pal2
        mpP_toral2=getSafeNodeDataPtr(mpPal2, \
NodeMechanicRotational::Torque);
        mpP_thetaal2=getSafeNodeDataPtr(mpPal2, \
NodeMechanicRotational::Angle);
        mpP_wal2=getSafeNodeDataPtr(mpPal2, \
NodeMechanicRotational::AngularVelocity);
        mpP_cal2=getSafeNodeDataPtr(mpPal2, \
NodeMechanicRotational::WaveVariable);
        mpP_Zcal2=getSafeNodeDataPtr(mpPal2, \
NodeMechanicRotational::CharImpedance);
        mpP_eqInertiaal2=getSafeNodeDataPtr(mpPal2, \
NodeMechanicRotational::EquivalentInertia);
        //Port Par2
        mpP_torar2=getSafeNodeDataPtr(mpPar2, \
NodeMechanicRotational::Torque);
        mpP_thetaar2=getSafeNodeDataPtr(mpPar2, \
NodeMechanicRotational::Angle);
        mpP_war2=getSafeNodeDataPtr(mpPar2, \
NodeMechanicRotational::AngularVelocity);
        mpP_car2=getSafeNodeDataPtr(mpPar2, \
NodeMechanicRotational::WaveVariable);
        mpP_Zcar2=getSafeNodeDataPtr(mpPar2, \
NodeMechanicRotational::CharImpedance);
        mpP_eqInertiaar2=getSafeNodeDataPtr(mpPar2, \
NodeMechanicRotational::EquivalentInertia);
        //Port Pfin
        mpP_torfin=getSafeNodeDataPtr(mpPfin, \
NodeMechanicRotational::Torque);
        mpP_thetafin=getSafeNodeDataPtr(mpPfin, \
NodeMechanicRotational::Angle);
        mpP_wfin=getSafeNodeDataPtr(mpPfin, \
NodeMechanicRotational::AngularVelocity);
        mpP_cfin=getSafeNodeDataPtr(mpPfin, \
NodeMechanicRotational::WaveVariable);
        mpP_Zcfin=getSafeNodeDataPtr(mpPfin, \
NodeMechanicRotational::CharImpedance);
        mpP_eqInertiafin=getSafeNodeDataPtr(mpPfin, \
NodeMechanicRotational::EquivalentInertia);

        //Read variables from nodes
        //Port Pal1
        toral1 = (*mpP_toral1);
        thetaal1 = (*mpP_thetaal1);
        wal1 = (*mpP_wal1);
        cal1 = (*mpP_cal1);
        Zcal1 = (*mpP_Zcal1);
        eqInertiaal1 = (*mpP_eqInertiaal1);
        //Port Par1
        torar1 = (*mpP_torar1);
        thetaar1 = (*mpP_thetaar1);
        war1 = (*mpP_war1);
        car1 = (*mpP_car1);
        Zcar1 = (*mpP_Zcar1);
        eqInertiaar1 = (*mpP_eqInertiaar1);
        //Port Pal12
        toral12 = (*mpP_toral12);
        thetaal12 = (*mpP_thetaal12);
        wal12 = (*mpP_wal12);
        cal12 = (*mpP_cal12);
        Zcal12 = (*mpP_Zcal12);
        eqInertiaal12 = (*mpP_eqInertiaal12);
        //Port Par12
        torar12 = (*mpP_torar12);
        thetaar12 = (*mpP_thetaar12);
        war12 = (*mpP_war12);
        car12 = (*mpP_car12);
        Zcar12 = (*mpP_Zcar12);
        eqInertiaar12 = (*mpP_eqInertiaar12);
        //Port Pal2
        toral2 = (*mpP_toral2);
        thetaal2 = (*mpP_thetaal2);
        wal2 = (*mpP_wal2);
        cal2 = (*mpP_cal2);
        Zcal2 = (*mpP_Zcal2);
        eqInertiaal2 = (*mpP_eqInertiaal2);
        //Port Par2
        torar2 = (*mpP_torar2);
        thetaar2 = (*mpP_thetaar2);
        war2 = (*mpP_war2);
        car2 = (*mpP_car2);
        Zcar2 = (*mpP_Zcar2);
        eqInertiaar2 = (*mpP_eqInertiaar2);
        //Port Pfin
        torfin = (*mpP_torfin);
        thetafin = (*mpP_thetafin);
        wfin = (*mpP_wfin);
        cfin = (*mpP_cfin);
        Zcfin = (*mpP_Zcfin);
        eqInertiafin = (*mpP_eqInertiafin);

        //Read inputVariables from nodes
        deCD = (*mpdeCD);
        deCY = (*mpdeCY);
        deCL = (*mpdeCL);
        deCl = (*mpdeCl);
        deCm = (*mpdeCm);
        deCn = (*mpdeCn);
        thrustl = (*mpthrustl);
        thrustr = (*mpthrustr);
        dezthrustl = (*mpdezthrustl);
        dezthrustr = (*mpdezthrustr);
        deythrustl = (*mpdeythrustl);
        deythrustr = (*mpdeythrustr);
        Mfuel = (*mpMfuel);
        Mcargo = (*mpMcargo);
        rho = (*mprho);
        vM = (*mpvM);
        vturbx = (*mpvturbx);
        vturby = (*mpvturby);
        vturbz = (*mpvturbz);
        wturbx = (*mpwturbx);
        wturby = (*mpwturby);
        wturbz = (*mpwturbz);

        //Read inputParameters from nodes
        afin = (*mpafin);
        an1 = (*mpan1);
        an2 = (*mpan2);
        ap1 = (*mpap1);
        ap2 = (*mpap2);
        AR1 = (*mpAR1);
        AR2 = (*mpAR2);
        ARfin = (*mpARfin);
        Cd01 = (*mpCd01);
        Cd02 = (*mpCd02);
        Cd0b = (*mpCd0b);
        Cd0fin = (*mpCd0fin);
        CdW0 = (*mpCdW0);
        CLalpha1 = (*mpCLalpha1);
        CLalpha2 = (*mpCLalpha2);
        CLalphabh = (*mpCLalphabh);
        CLalphabv = (*mpCLalphabv);
        CLalphafin = (*mpCLalphafin);
        CLde1 = (*mpCLde1);
        CLde12 = (*mpCLde12);
        Cdide1 = (*mpCdide1);
        Cdide12 = (*mpCdide12);
        Cdide112 = (*mpCdide112);
        de10 = (*mpde10);
        de120 = (*mpde120);
        dM = (*mpdM);
        Cm01 = (*mpCm01);
        Cmfs1 = (*mpCmfs1);
        Cmde1 = (*mpCmde1);
        Cmde12 = (*mpCmde12);
        CLdefin = (*mpCLdefin);
        Cydeelev = (*mpCydeelev);
        Cnbeta = (*mpCnbeta);
        dah1 = (*mpdah1);
        dah2 = (*mpdah2);
        en = (*mpen);
        e1 = (*mpe1);
        e2 = (*mpe2);
        efin = (*mpefin);
        epsM = (*mpepsM);
        awfin = (*mpawfin);
        awn1 = (*mpawn1);
        awn2 = (*mpawn2);
        awp1 = (*mpawp1);
        awp2 = (*mpawp2);
        gamma1 = (*mpgamma1);
        gamma2 = (*mpgamma2);
        hthrust0 = (*mphthrust0);
        ia1 = (*mpia1);
        ia2 = (*mpia2);
        Ix0 = (*mpIx0);
        Ixz0 = (*mpIxz0);
        Iy0 = (*mpIy0);
        Iz0 = (*mpIz0);
        lambda1 = (*mplambda1);
        lambda2 = (*mplambda2);
        lambdafin = (*mplambdafin);
        lc10 = (*mplc10);
        lc20 = (*mplc20);
        lc120 = (*mplc120);
        lcfin0 = (*mplcfin0);
        Me = (*mpMe);
        mac0 = (*mpmac0);
        rc10 = (*mprc10);
        rc120 = (*mprc120);
        rc20 = (*mprc20);
        rcfin0 = (*mprcfin0);
        S1 = (*mpS1);
        S20 = (*mpS20);
        Sbh0 = (*mpSbh0);
        Sbv0 = (*mpSbv0);
        Sfin0 = (*mpSfin0);
        xbach0 = (*mpxbach0);
        xbacv0 = (*mpxbacv0);
        xbcge0 = (*mpxbcge0);
        xcargo0 = (*mpxcargo0);
        xfuel0 = (*mpxfuel0);
        xw10 = (*mpxw10);
        xw20 = (*mpxw20);
        xwfin0 = (*mpxwfin0);
        xeng0 = (*mpxeng0);
        yeng0 = (*mpyeng0);
        g0 = (*mpg0);
        kground = (*mpkground);
        cground = (*mpcground);

        //Read outputVariables from nodes
        xcg = (*mpxcg);
        ycg = (*mpycg);
        zcg = (*mpzcg);
        vx = (*mpvx);
        vy = (*mpvy);
        vz = (*mpvz);
        Psi = (*mpPsi);
        Thetao = (*mpThetao);
        Phi = (*mpPhi);
        Ub = (*mpUb);
        Vb = (*mpVb);
        Wb = (*mpWb);
        v = (*mpv);
        Pb = (*mpPb);
        Qb = (*mpQb);
        Rb = (*mpRb);
        q0 = (*mpq0);
        q1 = (*mpq1);
        q2 = (*mpq2);
        q3 = (*mpq3);
        AlphaAttack = (*mpAlphaAttack);
        BetaSlip = (*mpBetaSlip);
        altitude = (*mpaltitude);
        gfx = (*mpgfx);
        gfy = (*mpgfy);
        gfz = (*mpgfz);
        CL1 = (*mpCL1);
        Cd1 = (*mpCd1);
        CL2 = (*mpCL2);
        Cd2 = (*mpCd2);
        CLtot = (*mpCLtot);
        Cdtot = (*mpCdtot);
        Lift1 = (*mpLift1);
        Lift2 = (*mpLift2);
        Fax = (*mpFax);
        Faz = (*mpFaz);

//==This code has been autogenerated using Compgen==
        //InitialExpressions
        q0 = Cos(Phi/2.)*Cos(Psi/2.)*Cos(Thetao/2.) + \
Sin(Phi/2.)*Sin(Psi/2.)*Sin(Thetao/2.);
        q1 = Cos(Psi/2.)*Cos(Thetao/2.)*Sin(Phi/2.) - \
Cos(Phi/2.)*Sin(Psi/2.)*Sin(Thetao/2.);
        q2 = Cos(Thetao/2.)*Sin(Phi/2.)*Sin(Psi/2.) + \
Cos(Phi/2.)*Cos(Psi/2.)*Sin(Thetao/2.);
        q3 = Cos(Phi/2.)*Cos(Thetao/2.)*Sin(Psi/2.) - \
Cos(Psi/2.)*Sin(Phi/2.)*Sin(Thetao/2.);

        //LocalExpressions
        b1 = Sqrt(AR1*S1);
        b2 = Sqrt(AR2*S2);
        smc = Sqrt(S1/AR1);
        mac = mac0*smc;
        hthrust = b1*hthrust0;
        Ix = AR1*Ix0*Me*S1;
        Ixz = Ixz0*Me*S1;
        Iy = (Iy0*Me*S1)/AR1;
        Iz = (Iz0*Me*S1)/AR1;
        lc1 = lc10*mac;
        lc2 = lc20*mac;
        lc12 = lc120*mac;
        lcfin = lcfin0*mac;
        rc1 = b1*rc10;
        rc12 = b1*rc120;
        rc2 = b1*rc20;
        rcfin = mac*rcfin0;
        S2 = S1*S20;
        Sbh = S1*Sbh0;
        Sbv = S1*Sbv0;
        Sfin = S1*Sfin0;
        xbach = mac*xbach0;
        xbacv = mac*xbacv0;
        xbcge = mac*xbcge0;
        xcargo = mac*xcargo0;
        xfuel = mac*xfuel0;
        xw1 = mac*xw10;
        xw2 = mac*xw20;
        xwfin = mac*xwfin0;
        xeng = mac*xeng0;
        yeng = b1*yeng0;
        betaM = Power(Power(1 - Power(v,2)/Power(vM,2),2) + \
(Power(epsM,2)*Power(v,2))/Power(vM,2),0.25);
        CLalpha1e = CLalpha1/betaM;
        CLalpha2e = CLalpha2/betaM;
        e1e = e1 - (e1*(1 - (0.31831*(-2 + 4*AR1*Power(Power(1 - \
Power(v,2)/Power(vM,2),2) + \
(Power(epsM,2)*Power(v,2))/Power(vM,2),0.25))*SecL(lambda1))/(Power(AR1,2)*Sq\
rt(Power(1 - Power(v,2)/Power(vM,2),2) + \
(Power(epsM,2)*Power(v,2))/Power(vM,2)))))/(1 + Power(en,(-2*(v - (1 - \
dM)*vM))/(dM*vM)));
        e2e = e2 - (e2*(1 - (0.31831*(-2 + 4*AR2*Power(Power(1 - \
Power(v,2)/Power(vM,2),2) + \
(Power(epsM,2)*Power(v,2))/Power(vM,2),0.25))*SecL(lambda2))/(Power(AR2,2)*Sq\
rt(Power(1 - Power(v,2)/Power(vM,2),2) + \
(Power(epsM,2)*Power(v,2))/Power(vM,2)))))/(1 + Power(en,(-2*(v - (1 - \
dM)*vM))/(dM*vM)));
        v = Sqrt(0.0001 + Power(Ub + (Power(q0,2) + Power(q1,2) - Power(q2,2) \
- Power(q3,2))*vturbx + 2*(q1*q2 - q0*q3)*vturby + 2*(q0*q2 + \
q1*q3)*vturbz,2) + Power(Vb + (Power(q0,2) + Power(q1,2) - Power(q2,2) - \
Power(q3,2))*vturbx + 2*(q1*q2 - q0*q3)*vturby + 2*(q0*q2 + q1*q3)*vturbz,2) \
+ Power(2*(-(q0*q2) + q1*q3)*vturbx + 2*(q0*q1 + q2*q3)*vturby + (Power(q0,2) \
- Power(q1,2) - Power(q2,2) + Power(q3,2))*vturbz + Wb,2));
        Alpha = Atan2L(2*(-(q0*q2) + q1*q3)*vturbx + 2*(q0*q1 + q2*q3)*vturby \
+ (Power(q0,2) - Power(q1,2) - Power(q2,2) + Power(q3,2))*vturbz + Wb,0.0001 \
+ Ub + (Power(q0,2) + Power(q1,2) - Power(q2,2) - Power(q3,2))*vturbx + \
2*(q1*q2 - q0*q3)*vturby + 2*(q0*q2 + q1*q3)*vturbz);
        qpress = (rho*Power(v,2))/2.;
        Beta = Atan2L(Vb + (Power(q0,2) + Power(q1,2) - Power(q2,2) - \
Power(q3,2))*vturbx + 2*(q1*q2 - q0*q3)*vturby + 2*(q0*q2 + \
q1*q3)*vturbz,Sqrt(0.0001 + Power(Ub + (Power(q0,2) + Power(q1,2) - \
Power(q2,2) - Power(q3,2))*vturbx + 2*(q1*q2 - q0*q3)*vturby + 2*(q0*q2 + \
q1*q3)*vturbz,2) + Power(2*(-(q0*q2) + q1*q3)*vturbx + 2*(q0*q1 + \
q2*q3)*vturby + (Power(q0,2) - Power(q1,2) - Power(q2,2) + \
Power(q3,2))*vturbz + Wb,2)));
        mass = Mcargo + Me + Mfuel;
        xbcg = (Me*xbcge + Mcargo*xcargo + Mfuel*xfuel)/mass;
        Dragl1 = qpress*S1*(Cd01/2. + Cdide1*Power(-de10 + thetaal1,2) - \
Cdide112*(-de10 + thetaal1)*(-de120 + thetaal12) + Cdide12*Power(-de120 + \
thetaal12,2) + CDragInd(Alpha*dah1 - \
ia1,AR1,e1e,CLalpha1e,ap1,an1,awp1,awn1)/2.);
        Dragr1 = qpress*S1*(Cd01/2. + Cdide1*Power(-de10 + thetaar1,2) - \
Cdide112*(-de10 + thetaar1)*(-de120 + thetaar12) + Cdide12*Power(-de120 + \
thetaar12,2) + CDragInd(Alpha*dah1 - \
ia1,AR1,e1e,CLalpha1e,ap1,an1,awp1,awn1)/2.);
        Liftl1 = qpress*S1*(CLde1*thetaal1 + CLde12*thetaal12 + \
CLift(Alpha*dah1 - ia1,CLalpha1e,ap1,an1,awp1,awn1)/2.);
        Liftr1 = qpress*S1*(CLde1*thetaar1 + CLde12*thetaar12 + \
CLift(Alpha*dah1 - ia1,CLalpha1e,ap1,an1,awp1,awn1)/2.);
        Lift1 = Liftl1 + Liftr1;
        Dragl2 = qpress*S2*(Cd02/2. + CDragInd(Alpha*dah2 - ia2 + \
thetaal2,AR2,e2e,CLalpha2e,ap2,an2,awp2,awn2)/2.);
        Dragr2 = qpress*S2*(Cd02/2. + CDragInd(Alpha*dah2 - ia2 + \
thetaar2,AR2,e2e,CLalpha2e,ap2,an2,awp2,awn2)/2.);
        Liftl2 = (qpress*S2*CLift(Alpha*dah2 - ia2 + \
thetaal2,CLalpha2e,ap2,an2,awp2,awn2))/2.;
        Liftr2 = (qpress*S2*CLift(Alpha*dah2 - ia2 + \
thetaar2,CLalpha2e,ap2,an2,awp2,awn2))/2.;
        Lift2 = Liftl2 + Liftr2;
        Liftb = CLalphabh*qpress*Sbh*Sin(Alpha);
        Dragb = qpress*((CdW0*S1)/(1 + Power(en,(-2*(v - (1 - \
dM)*vM))/(dM*vM))) + Cd0b*Sbh);
        Cfin = qpress*Sfin*CLift(-Beta - \
(CLdefin*thetafin)/CLalphafin,CLalphafin,afin,afin,awfin,awfin);
        Dragfin = Cd0fin*qpress*Sfin;
        Mdvtheta = (CLalpha2e*qpress*S2*Power(-xbcg + xw2,2))/(0.1 + v);
        Mdvpsi = (CLalphafin*qpress*Sfin*Power(-xbcg + xwfin,2))/(0.1 + v);
        Fx = (-Dragb - Dragfin - Dragl1 - Dragl2 - Dragr1 - Dragr2 - \
deCD*qpress*S1)*Cos(Alpha)*Cos(Beta) + \
thrustl*Cos(deythrustl)*Cos(dezthrustr) + \
thrustr*Cos(deythrustr)*Cos(dezthrustr) - (-Liftb - Liftl1 - Liftl2 - Liftr1 \
- Liftr2 - 2*deCL*qpress*S1)*Sin(Alpha) - Cos(Alpha)*Sin(Beta)*(-Cfin - \
2*deCY*qpress*S1 - Cydeelev*qpress*S2*(thetaal2 - thetaar2) + \
CLalphabv*qpress*Sbv*Sin(Beta));
        Fy = (-Dragb - Dragfin - Dragl1 - Dragl2 - Dragr1 - Dragr2 - \
deCD*qpress*S1)*Sin(Beta) + Cos(Beta)*(-Cfin - 2*deCY*qpress*S1 - \
Cydeelev*qpress*S2*(thetaal2 - thetaar2) + CLalphabv*qpress*Sbv*Sin(Beta)) - \
thrustl*Cos(dezthrustr)*Sin(deythrustl) - \
thrustr*Cos(dezthrustr)*Sin(deythrustr);
        Fz = (-Liftb - Liftl1 - Liftl2 - Liftr1 - Liftr2 - \
2*deCL*qpress*S1)*Cos(Alpha) - kground*zcg*onPositive(zcg) + (-Dragb - \
Dragfin - Dragl1 - Dragl2 - Dragr1 - Dragr2 - \
deCD*qpress*S1)*Cos(Beta)*Sin(Alpha) - Sin(Alpha)*Sin(Beta)*(-Cfin - \
2*deCY*qpress*S1 - Cydeelev*qpress*S2*(thetaal2 - thetaar2) + \
CLalphabv*qpress*Sbv*Sin(Beta)) - thrustl*Cos(deythrustl)*Sin(dezthrustr) - \
thrustr*Cos(deythrustr)*Sin(dezthrustr);
        Lb = (Liftl1 - Liftr1)*rc1 + (Liftl2 - Liftr2)*rc2 + 2*deCl*qpress*S1 \
- (0.12249999999999998*qpress*(Power(b1,2)*CLalpha1e*S1 + \
Power(b2,2)*CLalpha2e*S2)*(Pb + (Power(q0,2) + Power(q1,2) - Power(q2,2) - \
Power(q3,2))*wturbx + 2*(q1*q2 - q0*q3)*wturby + 2*(q0*q2 + \
q1*q3)*wturbz))/(0.1 + v) + yeng*(thrustl*Cos(deythrustl)*Sin(dezthrustl) - \
thrustr*Cos(deythrustr)*Sin(dezthrustr));
        Mb = deCm*qpress*S1 + qpress*S1*(-(Cmde1*thetaal1) - Cmde12*thetaal12 \
- Cmde1*thetaar1 - Cmde12*thetaar12 - (smc*(CLde1*thetaal1 + CLde12*thetaal12 \
+ CLift(Alpha*dah1 - ia1,CLalpha1e,ap1,an1,awp1,awn1)/2.))/(4.*(1 + \
Power(en,(-2*(v - (1 - dM)*vM))/(dM*vM)))) - (smc*(CLde1*thetaar1 + \
CLde12*thetaar12 + CLift(Alpha*dah1 - \
ia1,CLalpha1e,ap1,an1,awp1,awn1)/2.))/(4.*(1 + Power(en,(-2*(v - (1 - \
dM)*vM))/(dM*vM)))) + CMoment(Alpha*dah1 - ia1,Cm01,Cmfs1,ap1,an1,awp1,awn1)) \
+ (xbcg - xw1)*((Liftl1 + Liftr1)*Cos(Alpha) + (Dragl1 + Dragr1)*Sin(Alpha)) \
+ (xbcg - xw2)*((Liftl2 + Liftr2)*Cos(Alpha) - (Dragl2 + Dragr2)*Sin(Alpha)) \
+ (xbcg - xeng)*(thrustl*Cos(deythrustl)*Sin(dezthrustl) + \
thrustr*Cos(deythrustr)*Sin(dezthrustr));
        Nb = (-Dragl1 + Dragr1)*rc1 + (-Dragl2 + Dragr2)*rc2 + \
Beta*Cnbeta*deCn*qpress*S1 - Cfin*(-xbcg + xwfin) + (xbcg - \
xeng)*(thrustl*Cos(dezthrustl)*Sin(deythrustl) + \
thrustr*Cos(dezthrustr)*Sin(deythrustr));

        //Initialize delays
        delayParts1[1] = (-(Fx*mTimestep) + 2*g0*mass*mTimestep*q0*q2 - \
2*g0*mass*mTimestep*q1*q3 - 2*mass*Ub - mass*mTimestep*Rb*Vb + \
mass*mTimestep*Qb*Wb - 2*kground*mTimestep*q0*q2*zcg*onPositive(zcg) + \
2*kground*mTimestep*q1*q3*zcg*onPositive(zcg))/(2.*mass);
        mDelayedPart11.initialize(mNstep,delayParts1[1]);
        delayParts2[1] = (-(Fy*mTimestep) - 2*g0*mass*mTimestep*q0*q1 - \
2*g0*mass*mTimestep*q2*q3 + mass*mTimestep*Rb*Ub - 2*mass*Vb - \
mass*mTimestep*Pb*Wb + 2*kground*mTimestep*q0*q1*zcg*onPositive(zcg) + \
2*kground*mTimestep*q2*q3*zcg*onPositive(zcg))/(2.*mass);
        mDelayedPart21.initialize(mNstep,delayParts2[1]);
        delayParts3[1] = (-(Fz*mTimestep) - g0*mass*mTimestep*Power(q0,2) + \
g0*mass*mTimestep*Power(q1,2) + g0*mass*mTimestep*Power(q2,2) - \
g0*mass*mTimestep*Power(q3,2) - mass*mTimestep*Qb*Ub + mass*mTimestep*Pb*Vb - \
2*mass*Wb + kground*mTimestep*Power(q0,2)*zcg*onPositive(zcg) - \
kground*mTimestep*Power(q1,2)*zcg*onPositive(zcg) - \
kground*mTimestep*Power(q2,2)*zcg*onPositive(zcg) + \
kground*mTimestep*Power(q3,2)*zcg*onPositive(zcg))/(2.*mass);
        mDelayedPart31.initialize(mNstep,delayParts3[1]);
        delayParts4[1] = (Iz*Lb*mTimestep + Ixz*mTimestep*Nb - \
2*Power(Ixz,2)*Pb + 2*Ix*Iz*Pb + Ix*Ixz*mTimestep*Pb*Qb - \
Ixz*Iy*mTimestep*Pb*Qb + Ixz*Iz*mTimestep*Pb*Qb - \
Power(Ixz,2)*mTimestep*Qb*Rb + Iy*Iz*mTimestep*Qb*Rb - \
Power(Iz,2)*mTimestep*Qb*Rb)/(2*Power(Ixz,2) - 2*Ix*Iz + Ix*Ixz*mTimestep*Qb \
- Ixz*Iy*mTimestep*Qb + Ixz*Iz*mTimestep*Qb);
        mDelayedPart41.initialize(mNstep,delayParts4[1]);
        delayParts5[1] = (-(Mb*mTimestep) + Ixz*mTimestep*Power(Pb,2) - \
2*Iy*Qb + Ix*mTimestep*Pb*Rb - Iz*mTimestep*Pb*Rb - \
Ixz*mTimestep*Power(Rb,2))/(2.*Iy);
        mDelayedPart51.initialize(mNstep,delayParts5[1]);
        delayParts6[1] = (Ixz*Lb*mTimestep + Ix*mTimestep*Nb + \
Power(Ix,2)*mTimestep*Pb*Qb + Power(Ixz,2)*mTimestep*Pb*Qb - \
Ix*Iy*mTimestep*Pb*Qb - 2*Power(Ixz,2)*Rb + 2*Ix*Iz*Rb - \
Ix*Ixz*mTimestep*Qb*Rb + Ixz*Iy*mTimestep*Qb*Rb - \
Ixz*Iz*mTimestep*Qb*Rb)/(2*Power(Ixz,2) - 2*Ix*Iz - Ix*Ixz*mTimestep*Qb + \
Ixz*Iy*mTimestep*Qb - Ixz*Iz*mTimestep*Qb);
        mDelayedPart61.initialize(mNstep,delayParts6[1]);
        delayParts7[1] = (-4*q0 + mTimestep*Pb*q1 + mTimestep*q2*Qb + \
mTimestep*q3*Rb)/4.;
        mDelayedPart71.initialize(mNstep,delayParts7[1]);
        delayParts8[1] = (-(mTimestep*Pb*q0) - 4*q1 + mTimestep*q3*Qb - \
mTimestep*q2*Rb)/4.;
        mDelayedPart81.initialize(mNstep,delayParts8[1]);
        delayParts9[1] = (-4*q2 - mTimestep*Pb*q3 - mTimestep*q0*Qb + \
mTimestep*q1*Rb)/4.;
        mDelayedPart91.initialize(mNstep,delayParts9[1]);
        delayParts10[1] = (mTimestep*Pb*q2 - 4*q3 - mTimestep*q1*Qb - \
mTimestep*q0*Rb)/4.;
        mDelayedPart101.initialize(mNstep,delayParts10[1]);
        delayParts11[1] = (-(mTimestep*Power(q0,2)*Ub) - \
mTimestep*Power(q1,2)*Ub + mTimestep*Power(q2,2)*Ub + \
mTimestep*Power(q3,2)*Ub - 2*mTimestep*q1*q2*Vb + 2*mTimestep*q0*q3*Vb - \
2*mTimestep*q0*q2*Wb - 2*mTimestep*q1*q3*Wb - 2*xcg)/2.;
        mDelayedPart111.initialize(mNstep,delayParts11[1]);
        delayParts12[1] = (-2*mTimestep*q1*q2*Ub - 2*mTimestep*q0*q3*Ub - \
mTimestep*Power(q0,2)*Vb + mTimestep*Power(q1,2)*Vb - \
mTimestep*Power(q2,2)*Vb + mTimestep*Power(q3,2)*Vb + 2*mTimestep*q0*q1*Wb - \
2*mTimestep*q2*q3*Wb - 2*ycg)/2.;
        mDelayedPart121.initialize(mNstep,delayParts12[1]);
        delayParts13[1] = (2*mTimestep*q0*q2*Ub - 2*mTimestep*q1*q3*Ub - \
2*mTimestep*q0*q1*Vb - 2*mTimestep*q2*q3*Vb - mTimestep*Power(q0,2)*Wb + \
mTimestep*Power(q1,2)*Wb + mTimestep*Power(q2,2)*Wb - \
mTimestep*Power(q3,2)*Wb - 2*zcg)/2.;
        mDelayedPart131.initialize(mNstep,delayParts13[1]);

        delayedPart[1][1] = delayParts1[1];
        delayedPart[2][1] = delayParts2[1];
        delayedPart[3][1] = delayParts3[1];
        delayedPart[4][1] = delayParts4[1];
        delayedPart[5][1] = delayParts5[1];
        delayedPart[6][1] = delayParts6[1];
        delayedPart[7][1] = delayParts7[1];
        delayedPart[8][1] = delayParts8[1];
        delayedPart[9][1] = delayParts9[1];
        delayedPart[10][1] = delayParts10[1];
        delayedPart[11][1] = delayParts11[1];
        delayedPart[12][1] = delayParts12[1];
        delayedPart[13][1] = delayParts13[1];

        simulateOneTimestep();

     }
    void simulateOneTimestep()
     {
        Vec stateVar(13);
        Vec stateVark(13);
        Vec deltaStateVar(13);

        //Read variables from nodes
        //Port Pal1
        toral1 = (*mpP_toral1);
        thetaal1 = (*mpP_thetaal1);
        wal1 = (*mpP_wal1);
        eqInertiaal1 = (*mpP_eqInertiaal1);
        //Port Par1
        torar1 = (*mpP_torar1);
        thetaar1 = (*mpP_thetaar1);
        war1 = (*mpP_war1);
        eqInertiaar1 = (*mpP_eqInertiaar1);
        //Port Pal12
        toral12 = (*mpP_toral12);
        thetaal12 = (*mpP_thetaal12);
        wal12 = (*mpP_wal12);
        eqInertiaal12 = (*mpP_eqInertiaal12);
        //Port Par12
        torar12 = (*mpP_torar12);
        thetaar12 = (*mpP_thetaar12);
        war12 = (*mpP_war12);
        eqInertiaar12 = (*mpP_eqInertiaar12);
        //Port Pal2
        toral2 = (*mpP_toral2);
        thetaal2 = (*mpP_thetaal2);
        wal2 = (*mpP_wal2);
        eqInertiaal2 = (*mpP_eqInertiaal2);
        //Port Par2
        torar2 = (*mpP_torar2);
        thetaar2 = (*mpP_thetaar2);
        war2 = (*mpP_war2);
        eqInertiaar2 = (*mpP_eqInertiaar2);
        //Port Pfin
        torfin = (*mpP_torfin);
        thetafin = (*mpP_thetafin);
        wfin = (*mpP_wfin);
        eqInertiafin = (*mpP_eqInertiafin);

        //Read inputVariables from nodes
        deCD = (*mpdeCD);
        deCY = (*mpdeCY);
        deCL = (*mpdeCL);
        deCl = (*mpdeCl);
        deCm = (*mpdeCm);
        deCn = (*mpdeCn);
        thrustl = (*mpthrustl);
        thrustr = (*mpthrustr);
        dezthrustl = (*mpdezthrustl);
        dezthrustr = (*mpdezthrustr);
        deythrustl = (*mpdeythrustl);
        deythrustr = (*mpdeythrustr);
        Mfuel = (*mpMfuel);
        Mcargo = (*mpMcargo);
        rho = (*mprho);
        vM = (*mpvM);
        vturbx = (*mpvturbx);
        vturby = (*mpvturby);
        vturbz = (*mpvturbz);
        wturbx = (*mpwturbx);
        wturby = (*mpwturby);
        wturbz = (*mpwturbz);

        //Read inputParameters from nodes
        afin = (*mpafin);
        an1 = (*mpan1);
        an2 = (*mpan2);
        ap1 = (*mpap1);
        ap2 = (*mpap2);
        AR1 = (*mpAR1);
        AR2 = (*mpAR2);
        ARfin = (*mpARfin);
        Cd01 = (*mpCd01);
        Cd02 = (*mpCd02);
        Cd0b = (*mpCd0b);
        Cd0fin = (*mpCd0fin);
        CdW0 = (*mpCdW0);
        CLalpha1 = (*mpCLalpha1);
        CLalpha2 = (*mpCLalpha2);
        CLalphabh = (*mpCLalphabh);
        CLalphabv = (*mpCLalphabv);
        CLalphafin = (*mpCLalphafin);
        CLde1 = (*mpCLde1);
        CLde12 = (*mpCLde12);
        Cdide1 = (*mpCdide1);
        Cdide12 = (*mpCdide12);
        Cdide112 = (*mpCdide112);
        de10 = (*mpde10);
        de120 = (*mpde120);
        dM = (*mpdM);
        Cm01 = (*mpCm01);
        Cmfs1 = (*mpCmfs1);
        Cmde1 = (*mpCmde1);
        Cmde12 = (*mpCmde12);
        CLdefin = (*mpCLdefin);
        Cydeelev = (*mpCydeelev);
        Cnbeta = (*mpCnbeta);
        dah1 = (*mpdah1);
        dah2 = (*mpdah2);
        en = (*mpen);
        e1 = (*mpe1);
        e2 = (*mpe2);
        efin = (*mpefin);
        epsM = (*mpepsM);
        awfin = (*mpawfin);
        awn1 = (*mpawn1);
        awn2 = (*mpawn2);
        awp1 = (*mpawp1);
        awp2 = (*mpawp2);
        gamma1 = (*mpgamma1);
        gamma2 = (*mpgamma2);
        hthrust0 = (*mphthrust0);
        ia1 = (*mpia1);
        ia2 = (*mpia2);
        Ix0 = (*mpIx0);
        Ixz0 = (*mpIxz0);
        Iy0 = (*mpIy0);
        Iz0 = (*mpIz0);
        lambda1 = (*mplambda1);
        lambda2 = (*mplambda2);
        lambdafin = (*mplambdafin);
        lc10 = (*mplc10);
        lc20 = (*mplc20);
        lc120 = (*mplc120);
        lcfin0 = (*mplcfin0);
        Me = (*mpMe);
        mac0 = (*mpmac0);
        rc10 = (*mprc10);
        rc120 = (*mprc120);
        rc20 = (*mprc20);
        rcfin0 = (*mprcfin0);
        S1 = (*mpS1);
        S20 = (*mpS20);
        Sbh0 = (*mpSbh0);
        Sbv0 = (*mpSbv0);
        Sfin0 = (*mpSfin0);
        xbach0 = (*mpxbach0);
        xbacv0 = (*mpxbacv0);
        xbcge0 = (*mpxbcge0);
        xcargo0 = (*mpxcargo0);
        xfuel0 = (*mpxfuel0);
        xw10 = (*mpxw10);
        xw20 = (*mpxw20);
        xwfin0 = (*mpxwfin0);
        xeng0 = (*mpxeng0);
        yeng0 = (*mpyeng0);
        g0 = (*mpg0);
        kground = (*mpkground);
        cground = (*mpcground);

        //LocalExpressions
        b1 = Sqrt(AR1*S1);
        b2 = Sqrt(AR2*S2);
        smc = Sqrt(S1/AR1);
        mac = mac0*smc;
        hthrust = b1*hthrust0;
        Ix = AR1*Ix0*Me*S1;
        Ixz = Ixz0*Me*S1;
        Iy = (Iy0*Me*S1)/AR1;
        Iz = (Iz0*Me*S1)/AR1;
        lc1 = lc10*mac;
        lc2 = lc20*mac;
        lc12 = lc120*mac;
        lcfin = lcfin0*mac;
        rc1 = b1*rc10;
        rc12 = b1*rc120;
        rc2 = b1*rc20;
        rcfin = mac*rcfin0;
        S2 = S1*S20;
        Sbh = S1*Sbh0;
        Sbv = S1*Sbv0;
        Sfin = S1*Sfin0;
        xbach = mac*xbach0;
        xbacv = mac*xbacv0;
        xbcge = mac*xbcge0;
        xcargo = mac*xcargo0;
        xfuel = mac*xfuel0;
        xw1 = mac*xw10;
        xw2 = mac*xw20;
        xwfin = mac*xwfin0;
        xeng = mac*xeng0;
        yeng = b1*yeng0;
        betaM = Power(Power(1 - Power(v,2)/Power(vM,2),2) + \
(Power(epsM,2)*Power(v,2))/Power(vM,2),0.25);
        CLalpha1e = CLalpha1/betaM;
        CLalpha2e = CLalpha2/betaM;
        e1e = e1 - (e1*(1 - (0.31831*(-2 + 4*AR1*Power(Power(1 - \
Power(v,2)/Power(vM,2),2) + \
(Power(epsM,2)*Power(v,2))/Power(vM,2),0.25))*SecL(lambda1))/(Power(AR1,2)*Sq\
rt(Power(1 - Power(v,2)/Power(vM,2),2) + \
(Power(epsM,2)*Power(v,2))/Power(vM,2)))))/(1 + Power(en,(-2*(v - (1 - \
dM)*vM))/(dM*vM)));
        e2e = e2 - (e2*(1 - (0.31831*(-2 + 4*AR2*Power(Power(1 - \
Power(v,2)/Power(vM,2),2) + \
(Power(epsM,2)*Power(v,2))/Power(vM,2),0.25))*SecL(lambda2))/(Power(AR2,2)*Sq\
rt(Power(1 - Power(v,2)/Power(vM,2),2) + \
(Power(epsM,2)*Power(v,2))/Power(vM,2)))))/(1 + Power(en,(-2*(v - (1 - \
dM)*vM))/(dM*vM)));
        v = Sqrt(0.0001 + Power(Ub + (Power(q0,2) + Power(q1,2) - Power(q2,2) \
- Power(q3,2))*vturbx + 2*(q1*q2 - q0*q3)*vturby + 2*(q0*q2 + \
q1*q3)*vturbz,2) + Power(Vb + (Power(q0,2) + Power(q1,2) - Power(q2,2) - \
Power(q3,2))*vturbx + 2*(q1*q2 - q0*q3)*vturby + 2*(q0*q2 + q1*q3)*vturbz,2) \
+ Power(2*(-(q0*q2) + q1*q3)*vturbx + 2*(q0*q1 + q2*q3)*vturby + (Power(q0,2) \
- Power(q1,2) - Power(q2,2) + Power(q3,2))*vturbz + Wb,2));
        Alpha = Atan2L(2*(-(q0*q2) + q1*q3)*vturbx + 2*(q0*q1 + q2*q3)*vturby \
+ (Power(q0,2) - Power(q1,2) - Power(q2,2) + Power(q3,2))*vturbz + Wb,0.0001 \
+ Ub + (Power(q0,2) + Power(q1,2) - Power(q2,2) - Power(q3,2))*vturbx + \
2*(q1*q2 - q0*q3)*vturby + 2*(q0*q2 + q1*q3)*vturbz);
        qpress = (rho*Power(v,2))/2.;
        Beta = Atan2L(Vb + (Power(q0,2) + Power(q1,2) - Power(q2,2) - \
Power(q3,2))*vturbx + 2*(q1*q2 - q0*q3)*vturby + 2*(q0*q2 + \
q1*q3)*vturbz,Sqrt(0.0001 + Power(Ub + (Power(q0,2) + Power(q1,2) - \
Power(q2,2) - Power(q3,2))*vturbx + 2*(q1*q2 - q0*q3)*vturby + 2*(q0*q2 + \
q1*q3)*vturbz,2) + Power(2*(-(q0*q2) + q1*q3)*vturbx + 2*(q0*q1 + \
q2*q3)*vturby + (Power(q0,2) - Power(q1,2) - Power(q2,2) + \
Power(q3,2))*vturbz + Wb,2)));
        mass = Mcargo + Me + Mfuel;
        xbcg = (Me*xbcge + Mcargo*xcargo + Mfuel*xfuel)/mass;
        Dragl1 = qpress*S1*(Cd01/2. + Cdide1*Power(-de10 + thetaal1,2) - \
Cdide112*(-de10 + thetaal1)*(-de120 + thetaal12) + Cdide12*Power(-de120 + \
thetaal12,2) + CDragInd(Alpha*dah1 - \
ia1,AR1,e1e,CLalpha1e,ap1,an1,awp1,awn1)/2.);
        Dragr1 = qpress*S1*(Cd01/2. + Cdide1*Power(-de10 + thetaar1,2) - \
Cdide112*(-de10 + thetaar1)*(-de120 + thetaar12) + Cdide12*Power(-de120 + \
thetaar12,2) + CDragInd(Alpha*dah1 - \
ia1,AR1,e1e,CLalpha1e,ap1,an1,awp1,awn1)/2.);
        Liftl1 = qpress*S1*(CLde1*thetaal1 + CLde12*thetaal12 + \
CLift(Alpha*dah1 - ia1,CLalpha1e,ap1,an1,awp1,awn1)/2.);
        Liftr1 = qpress*S1*(CLde1*thetaar1 + CLde12*thetaar12 + \
CLift(Alpha*dah1 - ia1,CLalpha1e,ap1,an1,awp1,awn1)/2.);
        Lift1 = Liftl1 + Liftr1;
        Dragl2 = qpress*S2*(Cd02/2. + CDragInd(Alpha*dah2 - ia2 + \
thetaal2,AR2,e2e,CLalpha2e,ap2,an2,awp2,awn2)/2.);
        Dragr2 = qpress*S2*(Cd02/2. + CDragInd(Alpha*dah2 - ia2 + \
thetaar2,AR2,e2e,CLalpha2e,ap2,an2,awp2,awn2)/2.);
        Liftl2 = (qpress*S2*CLift(Alpha*dah2 - ia2 + \
thetaal2,CLalpha2e,ap2,an2,awp2,awn2))/2.;
        Liftr2 = (qpress*S2*CLift(Alpha*dah2 - ia2 + \
thetaar2,CLalpha2e,ap2,an2,awp2,awn2))/2.;
        Lift2 = Liftl2 + Liftr2;
        Liftb = CLalphabh*qpress*Sbh*Sin(Alpha);
        Dragb = qpress*((CdW0*S1)/(1 + Power(en,(-2*(v - (1 - \
dM)*vM))/(dM*vM))) + Cd0b*Sbh);
        Cfin = qpress*Sfin*CLift(-Beta - \
(CLdefin*thetafin)/CLalphafin,CLalphafin,afin,afin,awfin,awfin);
        Dragfin = Cd0fin*qpress*Sfin;
        Mdvtheta = (CLalpha2e*qpress*S2*Power(-xbcg + xw2,2))/(0.1 + v);
        Mdvpsi = (CLalphafin*qpress*Sfin*Power(-xbcg + xwfin,2))/(0.1 + v);
        Fx = (-Dragb - Dragfin - Dragl1 - Dragl2 - Dragr1 - Dragr2 - \
deCD*qpress*S1)*Cos(Alpha)*Cos(Beta) + \
thrustl*Cos(deythrustl)*Cos(dezthrustr) + \
thrustr*Cos(deythrustr)*Cos(dezthrustr) - (-Liftb - Liftl1 - Liftl2 - Liftr1 \
- Liftr2 - 2*deCL*qpress*S1)*Sin(Alpha) - Cos(Alpha)*Sin(Beta)*(-Cfin - \
2*deCY*qpress*S1 - Cydeelev*qpress*S2*(thetaal2 - thetaar2) + \
CLalphabv*qpress*Sbv*Sin(Beta));
        Fy = (-Dragb - Dragfin - Dragl1 - Dragl2 - Dragr1 - Dragr2 - \
deCD*qpress*S1)*Sin(Beta) + Cos(Beta)*(-Cfin - 2*deCY*qpress*S1 - \
Cydeelev*qpress*S2*(thetaal2 - thetaar2) + CLalphabv*qpress*Sbv*Sin(Beta)) - \
thrustl*Cos(dezthrustr)*Sin(deythrustl) - \
thrustr*Cos(dezthrustr)*Sin(deythrustr);
        Fz = (-Liftb - Liftl1 - Liftl2 - Liftr1 - Liftr2 - \
2*deCL*qpress*S1)*Cos(Alpha) - kground*zcg*onPositive(zcg) + (-Dragb - \
Dragfin - Dragl1 - Dragl2 - Dragr1 - Dragr2 - \
deCD*qpress*S1)*Cos(Beta)*Sin(Alpha) - Sin(Alpha)*Sin(Beta)*(-Cfin - \
2*deCY*qpress*S1 - Cydeelev*qpress*S2*(thetaal2 - thetaar2) + \
CLalphabv*qpress*Sbv*Sin(Beta)) - thrustl*Cos(deythrustl)*Sin(dezthrustr) - \
thrustr*Cos(deythrustr)*Sin(dezthrustr);
        Lb = (Liftl1 - Liftr1)*rc1 + (Liftl2 - Liftr2)*rc2 + 2*deCl*qpress*S1 \
- (0.12249999999999998*qpress*(Power(b1,2)*CLalpha1e*S1 + \
Power(b2,2)*CLalpha2e*S2)*(Pb + (Power(q0,2) + Power(q1,2) - Power(q2,2) - \
Power(q3,2))*wturbx + 2*(q1*q2 - q0*q3)*wturby + 2*(q0*q2 + \
q1*q3)*wturbz))/(0.1 + v) + yeng*(thrustl*Cos(deythrustl)*Sin(dezthrustl) - \
thrustr*Cos(deythrustr)*Sin(dezthrustr));
        Mb = deCm*qpress*S1 + qpress*S1*(-(Cmde1*thetaal1) - Cmde12*thetaal12 \
- Cmde1*thetaar1 - Cmde12*thetaar12 - (smc*(CLde1*thetaal1 + CLde12*thetaal12 \
+ CLift(Alpha*dah1 - ia1,CLalpha1e,ap1,an1,awp1,awn1)/2.))/(4.*(1 + \
Power(en,(-2*(v - (1 - dM)*vM))/(dM*vM)))) - (smc*(CLde1*thetaar1 + \
CLde12*thetaar12 + CLift(Alpha*dah1 - \
ia1,CLalpha1e,ap1,an1,awp1,awn1)/2.))/(4.*(1 + Power(en,(-2*(v - (1 - \
dM)*vM))/(dM*vM)))) + CMoment(Alpha*dah1 - ia1,Cm01,Cmfs1,ap1,an1,awp1,awn1)) \
+ (xbcg - xw1)*((Liftl1 + Liftr1)*Cos(Alpha) + (Dragl1 + Dragr1)*Sin(Alpha)) \
+ (xbcg - xw2)*((Liftl2 + Liftr2)*Cos(Alpha) - (Dragl2 + Dragr2)*Sin(Alpha)) \
+ (xbcg - xeng)*(thrustl*Cos(deythrustl)*Sin(dezthrustl) + \
thrustr*Cos(deythrustr)*Sin(dezthrustr));
        Nb = (-Dragl1 + Dragr1)*rc1 + (-Dragl2 + Dragr2)*rc2 + \
Beta*Cnbeta*deCn*qpress*S1 - Cfin*(-xbcg + xwfin) + (xbcg - \
xeng)*(thrustl*Cos(dezthrustl)*Sin(deythrustl) + \
thrustr*Cos(dezthrustr)*Sin(deythrustr));

        //Initializing variable vector for Newton-Raphson
        stateVark[0] = Ub;
        stateVark[1] = Vb;
        stateVark[2] = Wb;
        stateVark[3] = Pb;
        stateVark[4] = Qb;
        stateVark[5] = Rb;
        stateVark[6] = q0;
        stateVark[7] = q1;
        stateVark[8] = q2;
        stateVark[9] = q3;
        stateVark[10] = xcg;
        stateVark[11] = ycg;
        stateVark[12] = zcg;

        //Iterative solution using Newton-Rapshson
        for(iter=1;iter<=mNoiter;iter++)
        {
         //Aircraft6DOFSS
         //Differential-algebraic system of equation parts

          //Assemble differential-algebraic equations
          systemEquations[0] =Ub - (mTimestep*(Fx + mass*(-2*g0*q0*q2 + \
2*g0*q1*q3 + Rb*Vb - Qb*Wb) + 2*kground*(q0*q2 - \
q1*q3)*zcg*onPositive(zcg)))/(2.*mass) + delayedPart[1][1];
          systemEquations[1] =Vb - (mTimestep*(Fy + mass*(2*g0*q0*q1 + \
2*g0*q2*q3 - Rb*Ub + Pb*Wb) - 2*kground*(q0*q1 + \
q2*q3)*zcg*onPositive(zcg)))/(2.*mass) + delayedPart[2][1];
          systemEquations[2] =Wb - (mTimestep*(Fz + mass*(g0*(Power(q0,2) - \
Power(q1,2) - Power(q2,2) + Power(q3,2)) + Qb*Ub - Pb*Vb) + \
kground*(-Power(q0,2) + Power(q1,2) + Power(q2,2) - \
Power(q3,2))*zcg*onPositive(zcg)))/(2.*mass) + delayedPart[3][1];
          systemEquations[3] =Pb + (mTimestep*(-(Power(Iz,2)*Qb*Rb) + Ixz*(Nb \
- Ixz*Qb*Rb) + Iz*(Lb + Iy*Qb*Rb)))/(2*Power(Ixz,2) - 2*Ix*Iz + Ixz*(Ix - Iy \
+ Iz)*mTimestep*Qb) + delayedPart[4][1];
          systemEquations[4] =Qb - (mTimestep*(Mb + (-Ix + Iz)*Pb*Rb + \
Ixz*(-Power(Pb,2) + Power(Rb,2))))/(2.*Iy) + delayedPart[5][1];
          systemEquations[5] =-((mTimestep*(Ixz*Lb + Power(Ixz,2)*Pb*Qb + \
Ix*(Nb + (Ix - Iy)*Pb*Qb)))/(-2*Power(Ixz,2) + 2*Ix*Iz + Ixz*(Ix - Iy + \
Iz)*mTimestep*Qb)) + Rb + delayedPart[6][1];
          systemEquations[6] =q0 + (mTimestep*(Pb*q1 + q2*Qb + q3*Rb))/4. + \
delayedPart[7][1];
          systemEquations[7] =q1 - (mTimestep*(Pb*q0 - q3*Qb + q2*Rb))/4. + \
delayedPart[8][1];
          systemEquations[8] =q2 - (mTimestep*(Pb*q3 + q0*Qb - q1*Rb))/4. + \
delayedPart[9][1];
          systemEquations[9] =q3 + (mTimestep*(Pb*q2 - q1*Qb - q0*Rb))/4. + \
delayedPart[10][1];
          systemEquations[10] =-(mTimestep*(Power(q0,2)*Ub + Power(q1,2)*Ub - \
(Power(q2,2) + Power(q3,2))*Ub + q0*(-2*q3*Vb + 2*q2*Wb) + 2*q1*(q2*Vb + \
q3*Wb)))/2. + xcg + delayedPart[11][1];
          systemEquations[11] =(mTimestep*(-2*q1*q2*Ub - 2*q0*q3*Ub - \
Power(q0,2)*Vb + Power(q1,2)*Vb - Power(q2,2)*Vb + Power(q3,2)*Vb + \
2*q0*q1*Wb - 2*q2*q3*Wb))/2. + ycg + delayedPart[12][1];
          systemEquations[12] =-(mTimestep*(-2*q0*q2*Ub + 2*q1*q3*Ub + \
2*q0*q1*Vb + 2*q2*q3*Vb + Power(q0,2)*Wb - Power(q1,2)*Wb - Power(q2,2)*Wb + \
Power(q3,2)*Wb))/2. + zcg + delayedPart[13][1];

          //Jacobian matrix
          jacobianMatrix[0][0] = 1;
          jacobianMatrix[0][1] = -(mTimestep*Rb)/2.;
          jacobianMatrix[0][2] = (mTimestep*Qb)/2.;
          jacobianMatrix[0][3] = 0;
          jacobianMatrix[0][4] = (mTimestep*Wb)/2.;
          jacobianMatrix[0][5] = -(mTimestep*Vb)/2.;
          jacobianMatrix[0][6] = -(mTimestep*(-2*g0*mass*q2 + \
2*kground*q2*zcg*onPositive(zcg)))/(2.*mass);
          jacobianMatrix[0][7] = -(mTimestep*(2*g0*mass*q3 - \
2*kground*q3*zcg*onPositive(zcg)))/(2.*mass);
          jacobianMatrix[0][8] = -(mTimestep*(-2*g0*mass*q0 + \
2*kground*q0*zcg*onPositive(zcg)))/(2.*mass);
          jacobianMatrix[0][9] = -(mTimestep*(2*g0*mass*q1 - \
2*kground*q1*zcg*onPositive(zcg)))/(2.*mass);
          jacobianMatrix[0][10] = 0;
          jacobianMatrix[0][11] = 0;
          jacobianMatrix[0][12] = -((kground*mTimestep*(q0*q2 - \
q1*q3)*onPositive(zcg))/mass);
          jacobianMatrix[1][0] = (mTimestep*Rb)/2.;
          jacobianMatrix[1][1] = 1;
          jacobianMatrix[1][2] = -(mTimestep*Pb)/2.;
          jacobianMatrix[1][3] = -(mTimestep*Wb)/2.;
          jacobianMatrix[1][4] = 0;
          jacobianMatrix[1][5] = (mTimestep*Ub)/2.;
          jacobianMatrix[1][6] = -(mTimestep*(2*g0*mass*q1 - \
2*kground*q1*zcg*onPositive(zcg)))/(2.*mass);
          jacobianMatrix[1][7] = -(mTimestep*(2*g0*mass*q0 - \
2*kground*q0*zcg*onPositive(zcg)))/(2.*mass);
          jacobianMatrix[1][8] = -(mTimestep*(2*g0*mass*q3 - \
2*kground*q3*zcg*onPositive(zcg)))/(2.*mass);
          jacobianMatrix[1][9] = -(mTimestep*(2*g0*mass*q2 - \
2*kground*q2*zcg*onPositive(zcg)))/(2.*mass);
          jacobianMatrix[1][10] = 0;
          jacobianMatrix[1][11] = 0;
          jacobianMatrix[1][12] = (kground*mTimestep*(q0*q1 + \
q2*q3)*onPositive(zcg))/mass;
          jacobianMatrix[2][0] = -(mTimestep*Qb)/2.;
          jacobianMatrix[2][1] = (mTimestep*Pb)/2.;
          jacobianMatrix[2][2] = 1;
          jacobianMatrix[2][3] = (mTimestep*Vb)/2.;
          jacobianMatrix[2][4] = -(mTimestep*Ub)/2.;
          jacobianMatrix[2][5] = 0;
          jacobianMatrix[2][6] = -(mTimestep*(2*g0*mass*q0 - \
2*kground*q0*zcg*onPositive(zcg)))/(2.*mass);
          jacobianMatrix[2][7] = -(mTimestep*(-2*g0*mass*q1 + \
2*kground*q1*zcg*onPositive(zcg)))/(2.*mass);
          jacobianMatrix[2][8] = -(mTimestep*(-2*g0*mass*q2 + \
2*kground*q2*zcg*onPositive(zcg)))/(2.*mass);
          jacobianMatrix[2][9] = -(mTimestep*(2*g0*mass*q3 - \
2*kground*q3*zcg*onPositive(zcg)))/(2.*mass);
          jacobianMatrix[2][10] = 0;
          jacobianMatrix[2][11] = 0;
          jacobianMatrix[2][12] = -(kground*mTimestep*(-Power(q0,2) + \
Power(q1,2) + Power(q2,2) - Power(q3,2))*onPositive(zcg))/(2.*mass);
          jacobianMatrix[3][0] = 0;
          jacobianMatrix[3][1] = 0;
          jacobianMatrix[3][2] = 0;
          jacobianMatrix[3][3] = 1;
          jacobianMatrix[3][4] = (mTimestep*(-(Power(Ixz,2)*Rb) + Iy*Iz*Rb - \
Power(Iz,2)*Rb))/(2*Power(Ixz,2) - 2*Ix*Iz + Ixz*(Ix - Iy + Iz)*mTimestep*Qb) \
- (Ixz*(Ix - Iy + Iz)*Power(mTimestep,2)*(-(Power(Iz,2)*Qb*Rb) + Ixz*(Nb - \
Ixz*Qb*Rb) + Iz*(Lb + Iy*Qb*Rb)))/Power(2*Power(Ixz,2) - 2*Ix*Iz + Ixz*(Ix - \
Iy + Iz)*mTimestep*Qb,2);
          jacobianMatrix[3][5] = (mTimestep*(-(Power(Ixz,2)*Qb) + Iy*Iz*Qb - \
Power(Iz,2)*Qb))/(2*Power(Ixz,2) - 2*Ix*Iz + Ixz*(Ix - Iy + \
Iz)*mTimestep*Qb);
          jacobianMatrix[3][6] = 0;
          jacobianMatrix[3][7] = 0;
          jacobianMatrix[3][8] = 0;
          jacobianMatrix[3][9] = 0;
          jacobianMatrix[3][10] = 0;
          jacobianMatrix[3][11] = 0;
          jacobianMatrix[3][12] = 0;
          jacobianMatrix[4][0] = 0;
          jacobianMatrix[4][1] = 0;
          jacobianMatrix[4][2] = 0;
          jacobianMatrix[4][3] = -(mTimestep*(-2*Ixz*Pb + (-Ix + \
Iz)*Rb))/(2.*Iy);
          jacobianMatrix[4][4] = 1;
          jacobianMatrix[4][5] = -(mTimestep*((-Ix + Iz)*Pb + \
2*Ixz*Rb))/(2.*Iy);
          jacobianMatrix[4][6] = 0;
          jacobianMatrix[4][7] = 0;
          jacobianMatrix[4][8] = 0;
          jacobianMatrix[4][9] = 0;
          jacobianMatrix[4][10] = 0;
          jacobianMatrix[4][11] = 0;
          jacobianMatrix[4][12] = 0;
          jacobianMatrix[5][0] = 0;
          jacobianMatrix[5][1] = 0;
          jacobianMatrix[5][2] = 0;
          jacobianMatrix[5][3] = -((mTimestep*(Power(Ixz,2)*Qb + Ix*(Ix - \
Iy)*Qb))/(-2*Power(Ixz,2) + 2*Ix*Iz + Ixz*(Ix - Iy + Iz)*mTimestep*Qb));
          jacobianMatrix[5][4] = -((mTimestep*(Power(Ixz,2)*Pb + Ix*(Ix - \
Iy)*Pb))/(-2*Power(Ixz,2) + 2*Ix*Iz + Ixz*(Ix - Iy + Iz)*mTimestep*Qb)) + \
(Ixz*(Ix - Iy + Iz)*Power(mTimestep,2)*(Ixz*Lb + Power(Ixz,2)*Pb*Qb + Ix*(Nb \
+ (Ix - Iy)*Pb*Qb)))/Power(-2*Power(Ixz,2) + 2*Ix*Iz + Ixz*(Ix - Iy + \
Iz)*mTimestep*Qb,2);
          jacobianMatrix[5][5] = 1;
          jacobianMatrix[5][6] = 0;
          jacobianMatrix[5][7] = 0;
          jacobianMatrix[5][8] = 0;
          jacobianMatrix[5][9] = 0;
          jacobianMatrix[5][10] = 0;
          jacobianMatrix[5][11] = 0;
          jacobianMatrix[5][12] = 0;
          jacobianMatrix[6][0] = 0;
          jacobianMatrix[6][1] = 0;
          jacobianMatrix[6][2] = 0;
          jacobianMatrix[6][3] = (mTimestep*q1)/4.;
          jacobianMatrix[6][4] = (mTimestep*q2)/4.;
          jacobianMatrix[6][5] = (mTimestep*q3)/4.;
          jacobianMatrix[6][6] = 1;
          jacobianMatrix[6][7] = (mTimestep*Pb)/4.;
          jacobianMatrix[6][8] = (mTimestep*Qb)/4.;
          jacobianMatrix[6][9] = (mTimestep*Rb)/4.;
          jacobianMatrix[6][10] = 0;
          jacobianMatrix[6][11] = 0;
          jacobianMatrix[6][12] = 0;
          jacobianMatrix[7][0] = 0;
          jacobianMatrix[7][1] = 0;
          jacobianMatrix[7][2] = 0;
          jacobianMatrix[7][3] = -(mTimestep*q0)/4.;
          jacobianMatrix[7][4] = (mTimestep*q3)/4.;
          jacobianMatrix[7][5] = -(mTimestep*q2)/4.;
          jacobianMatrix[7][6] = -(mTimestep*Pb)/4.;
          jacobianMatrix[7][7] = 1;
          jacobianMatrix[7][8] = -(mTimestep*Rb)/4.;
          jacobianMatrix[7][9] = (mTimestep*Qb)/4.;
          jacobianMatrix[7][10] = 0;
          jacobianMatrix[7][11] = 0;
          jacobianMatrix[7][12] = 0;
          jacobianMatrix[8][0] = 0;
          jacobianMatrix[8][1] = 0;
          jacobianMatrix[8][2] = 0;
          jacobianMatrix[8][3] = -(mTimestep*q3)/4.;
          jacobianMatrix[8][4] = -(mTimestep*q0)/4.;
          jacobianMatrix[8][5] = (mTimestep*q1)/4.;
          jacobianMatrix[8][6] = -(mTimestep*Qb)/4.;
          jacobianMatrix[8][7] = (mTimestep*Rb)/4.;
          jacobianMatrix[8][8] = 1;
          jacobianMatrix[8][9] = -(mTimestep*Pb)/4.;
          jacobianMatrix[8][10] = 0;
          jacobianMatrix[8][11] = 0;
          jacobianMatrix[8][12] = 0;
          jacobianMatrix[9][0] = 0;
          jacobianMatrix[9][1] = 0;
          jacobianMatrix[9][2] = 0;
          jacobianMatrix[9][3] = (mTimestep*q2)/4.;
          jacobianMatrix[9][4] = -(mTimestep*q1)/4.;
          jacobianMatrix[9][5] = -(mTimestep*q0)/4.;
          jacobianMatrix[9][6] = -(mTimestep*Rb)/4.;
          jacobianMatrix[9][7] = -(mTimestep*Qb)/4.;
          jacobianMatrix[9][8] = (mTimestep*Pb)/4.;
          jacobianMatrix[9][9] = 1;
          jacobianMatrix[9][10] = 0;
          jacobianMatrix[9][11] = 0;
          jacobianMatrix[9][12] = 0;
          jacobianMatrix[10][0] = -(mTimestep*(Power(q0,2) + Power(q1,2) - \
Power(q2,2) - Power(q3,2)))/2.;
          jacobianMatrix[10][1] = -(mTimestep*(2*q1*q2 - 2*q0*q3))/2.;
          jacobianMatrix[10][2] = -(mTimestep*(2*q0*q2 + 2*q1*q3))/2.;
          jacobianMatrix[10][3] = 0;
          jacobianMatrix[10][4] = 0;
          jacobianMatrix[10][5] = 0;
          jacobianMatrix[10][6] = -(mTimestep*(2*q0*Ub - 2*q3*Vb + \
2*q2*Wb))/2.;
          jacobianMatrix[10][7] = -(mTimestep*(2*q1*Ub + 2*(q2*Vb + \
q3*Wb)))/2.;
          jacobianMatrix[10][8] = -(mTimestep*(-2*q2*Ub + 2*q1*Vb + \
2*q0*Wb))/2.;
          jacobianMatrix[10][9] = -(mTimestep*(-2*q3*Ub - 2*q0*Vb + \
2*q1*Wb))/2.;
          jacobianMatrix[10][10] = 1;
          jacobianMatrix[10][11] = 0;
          jacobianMatrix[10][12] = 0;
          jacobianMatrix[11][0] = (mTimestep*(-2*q1*q2 - 2*q0*q3))/2.;
          jacobianMatrix[11][1] = (mTimestep*(-Power(q0,2) + Power(q1,2) - \
Power(q2,2) + Power(q3,2)))/2.;
          jacobianMatrix[11][2] = (mTimestep*(2*q0*q1 - 2*q2*q3))/2.;
          jacobianMatrix[11][3] = 0;
          jacobianMatrix[11][4] = 0;
          jacobianMatrix[11][5] = 0;
          jacobianMatrix[11][6] = (mTimestep*(-2*q3*Ub - 2*q0*Vb + \
2*q1*Wb))/2.;
          jacobianMatrix[11][7] = (mTimestep*(-2*q2*Ub + 2*q1*Vb + \
2*q0*Wb))/2.;
          jacobianMatrix[11][8] = (mTimestep*(-2*q1*Ub - 2*q2*Vb - \
2*q3*Wb))/2.;
          jacobianMatrix[11][9] = (mTimestep*(-2*q0*Ub + 2*q3*Vb - \
2*q2*Wb))/2.;
          jacobianMatrix[11][10] = 0;
          jacobianMatrix[11][11] = 1;
          jacobianMatrix[11][12] = 0;
          jacobianMatrix[12][0] = -(mTimestep*(-2*q0*q2 + 2*q1*q3))/2.;
          jacobianMatrix[12][1] = -(mTimestep*(2*q0*q1 + 2*q2*q3))/2.;
          jacobianMatrix[12][2] = -(mTimestep*(Power(q0,2) - Power(q1,2) - \
Power(q2,2) + Power(q3,2)))/2.;
          jacobianMatrix[12][3] = 0;
          jacobianMatrix[12][4] = 0;
          jacobianMatrix[12][5] = 0;
          jacobianMatrix[12][6] = -(mTimestep*(-2*q2*Ub + 2*q1*Vb + \
2*q0*Wb))/2.;
          jacobianMatrix[12][7] = -(mTimestep*(2*q3*Ub + 2*q0*Vb - \
2*q1*Wb))/2.;
          jacobianMatrix[12][8] = -(mTimestep*(-2*q0*Ub + 2*q3*Vb - \
2*q2*Wb))/2.;
          jacobianMatrix[12][9] = -(mTimestep*(2*q1*Ub + 2*q2*Vb + \
2*q3*Wb))/2.;
          jacobianMatrix[12][10] = 0;
          jacobianMatrix[12][11] = 0;
          jacobianMatrix[12][12] = 1;
//==This code has been autogenerated using Compgen==

          //Solving equation using LU-faktorisation
          mpSolver->solve(jacobianMatrix, systemEquations, stateVark, iter);
          Ub=stateVark[0];
          Vb=stateVark[1];
          Wb=stateVark[2];
          Pb=stateVark[3];
          Qb=stateVark[4];
          Rb=stateVark[5];
          q0=stateVark[6];
          q1=stateVark[7];
          q2=stateVark[8];
          q3=stateVark[9];
          xcg=stateVark[10];
          ycg=stateVark[11];
          zcg=stateVark[12];
        //Expressions
        cal1 = CLde1*lc1*qpress*S1*(Alpha*dah1 - ia1 + thetaal1);
        car1 = CLde1*lc1*qpress*S1*(Alpha*dah1 - ia1 + thetaar1);
        cal12 = CLde12*lc12*qpress*S1*(Alpha*dah1 - ia1 + thetaal12);
        car12 = CLde12*lc12*qpress*S1*(Alpha*dah1 - ia1 + thetaar12);
        cal2 = CLalpha2e*lc2*qpress*S2*(Alpha*dah2 - ia2 + thetaal2);
        car2 = CLalpha2e*lc2*qpress*S2*(Alpha*dah2 - ia2 + thetaar2);
        cfin = CLdefin*lcfin*qpress*Sfin*(Beta + thetafin);
        vx = (Power(q0,2) + Power(q1,2) - Power(q2,2) - Power(q3,2))*Ub + \
2*(q1*q2 - q0*q3)*Vb + 2*(q0*q2 + q1*q3)*Wb;
        vy = 2*(q1*q2 + q0*q3)*Ub + (Power(q0,2) - Power(q1,2) + Power(q2,2) \
- Power(q3,2))*Vb + 2*(-(q0*q1) + q2*q3)*Wb;
        vz = 2*(-(q0*q2) + q1*q3)*Ub + 2*(q0*q1 + q2*q3)*Vb + (Power(q0,2) - \
Power(q1,2) - Power(q2,2) + Power(q3,2))*Wb;
        AlphaAttack = Alpha;
        BetaSlip = Beta;
        altitude = -zcg;
        Phi = Atan2L(2*(q0*q1 + q2*q3),Power(q0,2) - Power(q1,2) - \
Power(q2,2) + Power(q3,2));
        Thetao = ArcSinL(2*(q0*q2 - q1*q3));
        Psi = Atan2L(2*(q1*q2 + q0*q3),Power(q0,2) + Power(q1,2) - \
Power(q2,2) - Power(q3,2));
        gfx = Fx/mass;
        gfy = Fy/mass;
        gfz = Fz/mass;
        Faz = -Liftb - Liftl1 - Liftl2 - Liftr1 - Liftr2 - 2*deCL*qpress*S1;
        Fax = -Dragb - Dragfin - Dragl1 - Dragl2 - Dragr1 - Dragr2 - \
deCD*qpress*S1;
        CL1 = CLde1*thetaal1 + CLde12*thetaal12 + CLde1*thetaar1 + \
CLde12*thetaar12 + CLift(Alpha*dah1 - ia1,CLalpha1e,ap1,an1,awp1,awn1);
        CL2 = CLift(Alpha*dah2 - ia2 + \
thetaal2,CLalpha2e,ap2,an2,awp2,awn2)/2. + CLift(Alpha*dah2 - ia2 + \
thetaar2,CLalpha2e,ap2,an2,awp2,awn2)/2.;
        CLtot = CLde1*thetaal1 + CLde12*thetaal12 + CLde1*thetaar1 + \
CLde12*thetaar12 + CLift(Alpha*dah1 - ia1,CLalpha1e,ap1,an1,awp1,awn1) + \
(S2*(CLift(Alpha*dah2 - ia2 + thetaal2,CLalpha2e,ap2,an2,awp2,awn2)/2. + \
CLift(Alpha*dah2 - ia2 + thetaar2,CLalpha2e,ap2,an2,awp2,awn2)/2.))/S1 + \
(CLalphabh*Sbh*Sin(Alpha))/S1;
        Cd1 = Cd01 + Cdide1*Power(-de10 + thetaal1,2) - Cdide112*(-de10 + \
thetaal1)*(-de120 + thetaal12) + Cdide12*Power(-de120 + thetaal12,2) + \
Cdide1*Power(-de10 + thetaar1,2) - Cdide112*(-de10 + thetaar1)*(-de120 + \
thetaar12) + Cdide12*Power(-de120 + thetaar12,2) + CDragInd(Alpha*dah1 - \
ia1,AR1,e1e,CLalpha1e,ap1,an1,awp1,awn1);
        Cd2 = Cd02 + CDragInd(Alpha*dah2 - ia2 + \
thetaal2,AR2,e2e,CLalpha2e,ap2,an2,awp2,awn2)/2. + CDragInd(Alpha*dah2 - ia2 \
+ thetaar2,AR2,e2e,CLalpha2e,ap2,an2,awp2,awn2)/2.;
        Cdtot = Cd01 + CdW0/(1 + Power(en,(-2*(v - (1 - dM)*vM))/(dM*vM))) + \
(Cd0b*Sbh)/S1 + (Cd0fin*Sfin)/S1 + Cdide1*Power(-de10 + thetaal1,2) - \
Cdide112*(-de10 + thetaal1)*(-de120 + thetaal12) + Cdide12*Power(-de120 + \
thetaal12,2) + Cdide1*Power(-de10 + thetaar1,2) - Cdide112*(-de10 + \
thetaar1)*(-de120 + thetaar12) + Cdide12*Power(-de120 + thetaar12,2) + \
CDragInd(Alpha*dah1 - ia1,AR1,e1e,CLalpha1e,ap1,an1,awp1,awn1) + (S2*(Cd02 + \
CDragInd(Alpha*dah2 - ia2 + thetaal2,AR2,e2e,CLalpha2e,ap2,an2,awp2,awn2)/2. \
+ CDragInd(Alpha*dah2 - ia2 + \
thetaar2,AR2,e2e,CLalpha2e,ap2,an2,awp2,awn2)/2.))/S1;
        Zcfin = CLdefin*lcfin*mTimestep*qpress*Sfin;
        Zcal1 = CLde1*lc1*mTimestep*qpress*S1;
        Zcar1 = CLde1*lc1*mTimestep*qpress*S1;
        Zcal12 = CLde12*lc12*mTimestep*qpress*S1;
        Zcar12 = CLde12*lc12*mTimestep*qpress*S1;
        Zcal2 = CLalpha2e*lc2*mTimestep*qpress*S2;
        Zcar2 = CLalpha2e*lc2*mTimestep*qpress*S2;
        }

        //Calculate the delayed parts
        delayParts1[1] = (-(Fx*mTimestep) + 2*g0*mass*mTimestep*q0*q2 - \
2*g0*mass*mTimestep*q1*q3 - 2*mass*Ub - mass*mTimestep*Rb*Vb + \
mass*mTimestep*Qb*Wb - 2*kground*mTimestep*q0*q2*zcg*onPositive(zcg) + \
2*kground*mTimestep*q1*q3*zcg*onPositive(zcg))/(2.*mass);
        delayParts2[1] = (-(Fy*mTimestep) - 2*g0*mass*mTimestep*q0*q1 - \
2*g0*mass*mTimestep*q2*q3 + mass*mTimestep*Rb*Ub - 2*mass*Vb - \
mass*mTimestep*Pb*Wb + 2*kground*mTimestep*q0*q1*zcg*onPositive(zcg) + \
2*kground*mTimestep*q2*q3*zcg*onPositive(zcg))/(2.*mass);
        delayParts3[1] = (-(Fz*mTimestep) - g0*mass*mTimestep*Power(q0,2) + \
g0*mass*mTimestep*Power(q1,2) + g0*mass*mTimestep*Power(q2,2) - \
g0*mass*mTimestep*Power(q3,2) - mass*mTimestep*Qb*Ub + mass*mTimestep*Pb*Vb - \
2*mass*Wb + kground*mTimestep*Power(q0,2)*zcg*onPositive(zcg) - \
kground*mTimestep*Power(q1,2)*zcg*onPositive(zcg) - \
kground*mTimestep*Power(q2,2)*zcg*onPositive(zcg) + \
kground*mTimestep*Power(q3,2)*zcg*onPositive(zcg))/(2.*mass);
        delayParts4[1] = (Iz*Lb*mTimestep + Ixz*mTimestep*Nb - \
2*Power(Ixz,2)*Pb + 2*Ix*Iz*Pb + Ix*Ixz*mTimestep*Pb*Qb - \
Ixz*Iy*mTimestep*Pb*Qb + Ixz*Iz*mTimestep*Pb*Qb - \
Power(Ixz,2)*mTimestep*Qb*Rb + Iy*Iz*mTimestep*Qb*Rb - \
Power(Iz,2)*mTimestep*Qb*Rb)/(2*Power(Ixz,2) - 2*Ix*Iz + Ix*Ixz*mTimestep*Qb \
- Ixz*Iy*mTimestep*Qb + Ixz*Iz*mTimestep*Qb);
        delayParts5[1] = (-(Mb*mTimestep) + Ixz*mTimestep*Power(Pb,2) - \
2*Iy*Qb + Ix*mTimestep*Pb*Rb - Iz*mTimestep*Pb*Rb - \
Ixz*mTimestep*Power(Rb,2))/(2.*Iy);
        delayParts6[1] = (Ixz*Lb*mTimestep + Ix*mTimestep*Nb + \
Power(Ix,2)*mTimestep*Pb*Qb + Power(Ixz,2)*mTimestep*Pb*Qb - \
Ix*Iy*mTimestep*Pb*Qb - 2*Power(Ixz,2)*Rb + 2*Ix*Iz*Rb - \
Ix*Ixz*mTimestep*Qb*Rb + Ixz*Iy*mTimestep*Qb*Rb - \
Ixz*Iz*mTimestep*Qb*Rb)/(2*Power(Ixz,2) - 2*Ix*Iz - Ix*Ixz*mTimestep*Qb + \
Ixz*Iy*mTimestep*Qb - Ixz*Iz*mTimestep*Qb);
        delayParts7[1] = (-4*q0 + mTimestep*Pb*q1 + mTimestep*q2*Qb + \
mTimestep*q3*Rb)/4.;
        delayParts8[1] = (-(mTimestep*Pb*q0) - 4*q1 + mTimestep*q3*Qb - \
mTimestep*q2*Rb)/4.;
        delayParts9[1] = (-4*q2 - mTimestep*Pb*q3 - mTimestep*q0*Qb + \
mTimestep*q1*Rb)/4.;
        delayParts10[1] = (mTimestep*Pb*q2 - 4*q3 - mTimestep*q1*Qb - \
mTimestep*q0*Rb)/4.;
        delayParts11[1] = (-(mTimestep*Power(q0,2)*Ub) - \
mTimestep*Power(q1,2)*Ub + mTimestep*Power(q2,2)*Ub + \
mTimestep*Power(q3,2)*Ub - 2*mTimestep*q1*q2*Vb + 2*mTimestep*q0*q3*Vb - \
2*mTimestep*q0*q2*Wb - 2*mTimestep*q1*q3*Wb - 2*xcg)/2.;
        delayParts12[1] = (-2*mTimestep*q1*q2*Ub - 2*mTimestep*q0*q3*Ub - \
mTimestep*Power(q0,2)*Vb + mTimestep*Power(q1,2)*Vb - \
mTimestep*Power(q2,2)*Vb + mTimestep*Power(q3,2)*Vb + 2*mTimestep*q0*q1*Wb - \
2*mTimestep*q2*q3*Wb - 2*ycg)/2.;
        delayParts13[1] = (2*mTimestep*q0*q2*Ub - 2*mTimestep*q1*q3*Ub - \
2*mTimestep*q0*q1*Vb - 2*mTimestep*q2*q3*Vb - mTimestep*Power(q0,2)*Wb + \
mTimestep*Power(q1,2)*Wb + mTimestep*Power(q2,2)*Wb - \
mTimestep*Power(q3,2)*Wb - 2*zcg)/2.;

        delayedPart[1][1] = delayParts1[1];
        delayedPart[2][1] = delayParts2[1];
        delayedPart[3][1] = delayParts3[1];
        delayedPart[4][1] = delayParts4[1];
        delayedPart[5][1] = delayParts5[1];
        delayedPart[6][1] = delayParts6[1];
        delayedPart[7][1] = delayParts7[1];
        delayedPart[8][1] = delayParts8[1];
        delayedPart[9][1] = delayParts9[1];
        delayedPart[10][1] = delayParts10[1];
        delayedPart[11][1] = delayParts11[1];
        delayedPart[12][1] = delayParts12[1];
        delayedPart[13][1] = delayParts13[1];

        //Write new values to nodes
        //Port Pal1
        (*mpP_cal1)=cal1;
        (*mpP_Zcal1)=Zcal1;
        //Port Par1
        (*mpP_car1)=car1;
        (*mpP_Zcar1)=Zcar1;
        //Port Pal12
        (*mpP_cal12)=cal12;
        (*mpP_Zcal12)=Zcal12;
        //Port Par12
        (*mpP_car12)=car12;
        (*mpP_Zcar12)=Zcar12;
        //Port Pal2
        (*mpP_cal2)=cal2;
        (*mpP_Zcal2)=Zcal2;
        //Port Par2
        (*mpP_car2)=car2;
        (*mpP_Zcar2)=Zcar2;
        //Port Pfin
        (*mpP_cfin)=cfin;
        (*mpP_Zcfin)=Zcfin;
        //outputVariables
        (*mpxcg)=xcg;
        (*mpycg)=ycg;
        (*mpzcg)=zcg;
        (*mpvx)=vx;
        (*mpvy)=vy;
        (*mpvz)=vz;
        (*mpPsi)=Psi;
        (*mpThetao)=Thetao;
        (*mpPhi)=Phi;
        (*mpUb)=Ub;
        (*mpVb)=Vb;
        (*mpWb)=Wb;
        (*mpv)=v;
        (*mpPb)=Pb;
        (*mpQb)=Qb;
        (*mpRb)=Rb;
        (*mpq0)=q0;
        (*mpq1)=q1;
        (*mpq2)=q2;
        (*mpq3)=q3;
        (*mpAlphaAttack)=AlphaAttack;
        (*mpBetaSlip)=BetaSlip;
        (*mpaltitude)=altitude;
        (*mpgfx)=gfx;
        (*mpgfy)=gfy;
        (*mpgfz)=gfz;
        (*mpCL1)=CL1;
        (*mpCd1)=Cd1;
        (*mpCL2)=CL2;
        (*mpCd2)=Cd2;
        (*mpCLtot)=CLtot;
        (*mpCdtot)=Cdtot;
        (*mpLift1)=Lift1;
        (*mpLift2)=Lift2;
        (*mpFax)=Fax;
        (*mpFaz)=Faz;

        //Update the delayed variabels
        mDelayedPart11.update(delayParts1[1]);
        mDelayedPart21.update(delayParts2[1]);
        mDelayedPart31.update(delayParts3[1]);
        mDelayedPart41.update(delayParts4[1]);
        mDelayedPart51.update(delayParts5[1]);
        mDelayedPart61.update(delayParts6[1]);
        mDelayedPart71.update(delayParts7[1]);
        mDelayedPart81.update(delayParts8[1]);
        mDelayedPart91.update(delayParts9[1]);
        mDelayedPart101.update(delayParts10[1]);
        mDelayedPart111.update(delayParts11[1]);
        mDelayedPart121.update(delayParts12[1]);
        mDelayedPart131.update(delayParts13[1]);

     }
    void deconfigure()
    {
        delete mpSolver;
    }
};
#endif // AEROAIRCRAFT6DOFSS_HPP_INCLUDED
